source: flair-src/trunk/tools/VrpnLite/src/VrpnLite.cpp @ 451

Last change on this file since 451 was 451, checked in by Sanahuja Guillaume, 2 months ago

up

File size: 7.4 KB
Line 
1//  created:    2019/03/12
2//  filename:   VrpnLite.cpp
3//
4//  author:     Guillaume Sanahuja
5//              Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7//  version:    $Id: $
8//
9//  purpose:    vrpnlite, to forward it to bth for exemple
10//              usefull to reduce vrpn frame size
11//
12/*********************************************************************/
13
14#include "VrpnLite.h"
15#include <VrpnClient.h>
16#include <VrpnObject.h>
17#include <Thread.h>
18#include <UdpSocket.h>
19#include <TabWidget.h>
20#include <Tab.h>
21#include <PushButton.h>
22#include <FrameworkManager.h>
23#include <Quaternion.h>
24#include <string.h>
25#include <bits/stdc++.h>
26
27using namespace std;
28using namespace flair::core;
29using namespace flair::sensor;
30using namespace flair::gui;
31
32//if we don'receive ack from client, do not send vrpnobjects anymore
33#define REMOVE_TIMEOUT (Time)2000000000
34
35
36VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
37  vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80); 
38  Tab* tab=new Tab(vrpnclient->GetTabWidget(),"Main");
39  killButton=new PushButton(tab->NewRow(),"kill");
40  dataSocket = new UdpSocket(this,"client socket",vrpnLitePort);
41  vrpnclient->Start();
42}
43
44VrpnLite::~VrpnLite() {
45}
46
47void VrpnLite::Run(void) {
48  Time dataSocketTimeout;
49  char msg[256];
50  int src_id;
51 
52  while (!ToBeStopped()) {
53    if(killButton->Clicked()) SafeStop();
54    //get last vrpnobject
55    VrpnObject* lastVrpnObject=NULL;
56    if(connections.size()!=0) {
57      if(connections.back().vrpnobjects.size()!=0) lastVrpnObject=lastVrpnObject=connections.back().vrpnobjects.back();
58    }
59   
60    if(lastVrpnObject!=NULL) {
61      //wait for last one to be sure all are up to date
62      //but if object is not tracked we send nothing... send it unsynchronized???
63      if(WaitUpdate(lastVrpnObject,100000000)) {
64          SendObjects();
65      }
66      dataSocketTimeout=TIME_NONBLOCK;
67    } else {
68      dataSocketTimeout=100000000;
69    }
70   
71    ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
72    connection_t* matchConnection=ConnectionOfSrcId(src_id);
73    //receive an ack
74    if(rcv==1) {
75      if(matchConnection!=NULL) matchConnection->lastAck=GetTime();
76      continue;//do not handle add or remove cases
77    }
78    if(rcv>0) {//add or remove
79      int16_t id;
80      string objectName=string(msg,rcv-sizeof(id));
81      memcpy(&id,&msg[rcv-sizeof(id)],sizeof(id));
82      dataSocket->HostToNetwork((char*)&id,sizeof(id));
83     
84      if(id>0) { //add object
85        if(matchConnection==NULL) {
86          connection_t connection;
87          connection.lastAck=GetTime();
88          connection.srcId=src_id;
89          connections.push_back(connection);
90          matchConnection=&(connections.back());
91        }
92        //assume we receive it in the good order
93        if(id==matchConnection->vrpnobjects.size()+1) {//id 0 is not used as we use positive and negative id for add/remove
94          Printf("adding object %s with id %i from client %i\n",objectName.c_str(),id,src_id);
95          VrpnObject* vrpnobject = new VrpnObject(objectName,vrpnclient->GetTabWidget());
96          matchConnection->vrpnobjects.push_back(vrpnobject);
97        }else {
98          Err("adding object %s failed, expected id %i, got %i\n",objectName.c_str(),matchConnection->vrpnobjects.size()+1,id);
99        }
100      } else { //remove object
101        for (auto it = matchConnection->vrpnobjects.begin();it < matchConnection->vrpnobjects.end(); it++) {
102          if ((*it)->ObjectName() == objectName) {
103            Printf("removing object %s with id %i from client %i\n",objectName.c_str(),-id,src_id);
104            delete (*it);
105            auto iteratorEnd=remove(matchConnection->vrpnobjects.begin(), matchConnection->vrpnobjects.end(),(*it));
106            matchConnection->vrpnobjects.erase(iteratorEnd, matchConnection->vrpnobjects.end());
107            if(matchConnection->vrpnobjects.size()==0) {
108              Printf("TODO: removing empty client %i\n",src_id);
109              //auto iteratorEnd=remove(connections.begin(), connections.end(),*matchConnection);
110              //connections.erase(iteratorEnd, connections.end());
111            }
112            break;
113          }
114        }
115      }
116    }
117  }
118}
119
120
121VrpnLite::connection_t* VrpnLite::ConnectionOfSrcId(uint16_t srcId) {
122  for (int i=0;i<connections.size();i++) {
123    if(connections.at(i).srcId==srcId) return &(connections.at(i));
124  }
125  return NULL;
126}
127
128void VrpnLite::SendObjects(void) {
129 
130  int16_t position[3];
131  int16_t quaternion[4];
132  Time time;
133 
134  for (int i=0;i<connections.size();i++) {
135      if(connections.at(i).lastAck+REMOVE_TIMEOUT<GetTime() && connections.at(i).vrpnobjects.size()!=0) {
136        Printf("client %i did not send ack from %ins, removing it\n",i,REMOVE_TIMEOUT);
137        for (auto it = connections.at(i).vrpnobjects.begin();it < connections.at(i).vrpnobjects.end(); it++) {
138          delete (*it);
139        }
140        connections.at(i).vrpnobjects.clear();
141      }
142     
143      //do not handle empty connections
144      //todo: remove empty connections
145      if(connections.at(i).vrpnobjects.size()==0) continue;
146     
147      char datas[connections.at(i).vrpnobjects.size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
148      char *datasPtr=datas;
149      for (auto it = connections.at(i).vrpnobjects.begin();it < connections.at(i).vrpnobjects.end(); it++) {
150        Vector3Df objectPosition;
151        Quaternion objectQuaternion;
152        const VrpnObject* vrpnobject=*it;
153//printf("%lld send to %s client %i size %i\n",GetTime()/1000000,vrpnobject->ObjectName().c_str(),connections.at(i).srcId,sizeof(datas));
154        vrpnobject->GetPosition(objectPosition);
155        vrpnobject->GetQuaternion(objectQuaternion);
156        time=vrpnobject->GetLastPacketTime();
157
158        position[0]=ConvertPosition(objectPosition.x);
159        position[1]=ConvertPosition(objectPosition.y);
160        position[2]=ConvertPosition(objectPosition.z);
161        quaternion[0]=ConvertQuaternion(objectQuaternion.q0);
162        quaternion[1]=ConvertQuaternion(objectQuaternion.q1);
163        quaternion[2]=ConvertQuaternion(objectQuaternion.q2);
164        quaternion[3]=ConvertQuaternion(objectQuaternion.q3);
165
166        for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
167        for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
168
169        memcpy(datasPtr,position, sizeof(position));
170        datasPtr+=sizeof(position);
171        memcpy(datasPtr,quaternion, sizeof(quaternion));
172        datasPtr+=sizeof(quaternion);
173      }
174      dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
175      memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!
176      dataSocket->SendMessage(datas,sizeof(datas),connections.at(i).srcId);
177      //printf("%lld send client %i size %i\n",GetTime()/1000000,connections.at(i).srcId,sizeof(datas));
178 
179  }
180}
181
182int16_t VrpnLite::ConvertQuaternion(float value) const{
183  int16_t tmp;
184  tmp=value*32767.;
185  if(value<-1) {
186    tmp=-32767;
187    Warn("position value is %f, saturating it to %i\n",value,tmp);
188  }
189  if(value>1) {
190    tmp=32767;
191    Warn("position value is %f, saturating it to %i\n",value,tmp);
192  }
193  return tmp;
194}
195
196int16_t VrpnLite::ConvertPosition(float value) const{
197  int16_t tmp;
198  tmp=value*1000;
199  if(value<-32.768) {
200    tmp=-32768;
201    Warn("position value is %f, saturating it to %i\n",value,tmp);
202  }
203  if(value>32.767) {
204    tmp=32767;
205    Warn("position value is %f, saturating it to %i\n",value,tmp);
206  }
207  return tmp;
208}
Note: See TracBrowser for help on using the repository browser.