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

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

change timeout value

File size: 7.5 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)5000000000
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 %lldns, removing it (last ack %lld, time %lld)\n",i,REMOVE_TIMEOUT,connections.at(i).lastAck,GetTime());
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.