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

Last change on this file since 456 was 454, checked in by Sanahuja Guillaume, 3 years ago

change timeout value

File size: 7.5 KB
RevLine 
[308]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>
[446]19#include <TabWidget.h>
20#include <Tab.h>
21#include <PushButton.h>
[308]22#include <FrameworkManager.h>
23#include <Quaternion.h>
24#include <string.h>
[447]25#include <bits/stdc++.h>
[308]26
27using namespace std;
28using namespace flair::core;
29using namespace flair::sensor;
[446]30using namespace flair::gui;
[308]31
[448]32//if we don'receive ack from client, do not send vrpnobjects anymore
[454]33#define REMOVE_TIMEOUT (Time)5000000000
[448]34
35
[330]36VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
37 vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80);
[446]38 Tab* tab=new Tab(vrpnclient->GetTabWidget(),"Main");
39 killButton=new PushButton(tab->NewRow(),"kill");
[330]40 dataSocket = new UdpSocket(this,"client socket",vrpnLitePort);
[308]41 vrpnclient->Start();
42}
43
44VrpnLite::~VrpnLite() {
45}
46
47void VrpnLite::Run(void) {
[330]48 Time dataSocketTimeout;
49 char msg[256];
[363]50 int src_id;
[330]51
[308]52 while (!ToBeStopped()) {
[446]53 if(killButton->Clicked()) SafeStop();
[448]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 }
[446]59
[448]60 if(lastVrpnObject!=NULL) {
[330]61 //wait for last one to be sure all are up to date
[334]62 //but if object is not tracked we send nothing... send it unsynchronized???
[448]63 if(WaitUpdate(lastVrpnObject,100000000)) {
[330]64 SendObjects();
65 }
66 dataSocketTimeout=TIME_NONBLOCK;
67 } else {
68 dataSocketTimeout=100000000;
[308]69 }
70
[363]71 ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
[448]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
[447]79 int16_t id;
[448]80 string objectName=string(msg,rcv-sizeof(id));
[447]81 memcpy(&id,&msg[rcv-sizeof(id)],sizeof(id));
[331]82 dataSocket->HostToNetwork((char*)&id,sizeof(id));
[448]83
[447]84 if(id>0) { //add object
[448]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());
[447]91 }
92 //assume we receive it in the good order
[448]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);
[447]97 }else {
[448]98 Err("adding object %s failed, expected id %i, got %i\n",objectName.c_str(),matchConnection->vrpnobjects.size()+1,id);
[447]99 }
100 } else { //remove object
[448]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());
[447]111 }
112 break;
113 }
114 }
[363]115 }
[330]116 }
[308]117 }
118}
119
[448]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) {
[364]129
130 int16_t position[3];
[330]131 int16_t quaternion[4];
132 Time time;
[448]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) {
[454]136 Printf("client %i did not send ack from %lldns, removing it (last ack %lld, time %lld)\n",i,REMOVE_TIMEOUT,connections.at(i).lastAck,GetTime());
[448]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
[451]143 //do not handle empty connections
144 //todo: remove empty connections
145 if(connections.at(i).vrpnobjects.size()==0) continue;
146
[448]147 char datas[connections.at(i).vrpnobjects.size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
[364]148 char *datasPtr=datas;
[448]149 for (auto it = connections.at(i).vrpnobjects.begin();it < connections.at(i).vrpnobjects.end(); it++) {
[363]150 Vector3Df objectPosition;
151 Quaternion objectQuaternion;
152 const VrpnObject* vrpnobject=*it;
[451]153//printf("%lld send to %s client %i size %i\n",GetTime()/1000000,vrpnobject->ObjectName().c_str(),connections.at(i).srcId,sizeof(datas));
[363]154 vrpnobject->GetPosition(objectPosition);
155 vrpnobject->GetQuaternion(objectQuaternion);
156 time=vrpnobject->GetLastPacketTime();
[330]157
[363]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);
[330]165
[363]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]));
[330]168
[363]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!
[450]176 dataSocket->SendMessage(datas,sizeof(datas),connections.at(i).srcId);
[451]177 //printf("%lld send client %i size %i\n",GetTime()/1000000,connections.at(i).srcId,sizeof(datas));
178
[330]179 }
[317]180}
[330]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;
[452]208}
Note: See TracBrowser for help on using the repository browser.