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

Last change on this file since 447 was 447, checked in by Sanahuja Guillaume, 2 years ago

vrpnlite handle disconection

File size: 6.3 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
32VrpnLite::VrpnLite(int vrpnLitePort,string vrpnServerAddress): Thread(getFrameworkManager(),"VrpnLite",90) {
33 vrpnclient=new VrpnClient("vrpn", vrpnServerAddress,80);
34 Tab* tab=new Tab(vrpnclient->GetTabWidget(),"Main");
35 killButton=new PushButton(tab->NewRow(),"kill");
36 dataSocket = new UdpSocket(this,"client socket",vrpnLitePort);
37 vrpnclient->Start();
38}
39
40VrpnLite::~VrpnLite() {
41}
42
43void VrpnLite::Run(void) {
44 Time dataSocketTimeout;
45 char msg[256];
46 int src_id;
47
48 while (!ToBeStopped()) {
49 if(killButton->Clicked()) SafeStop();
50
51 if(vrpnobjects.size()>0) {
52 //wait for last one to be sure all are up to date
53 //but if object is not tracked we send nothing... send it unsynchronized???
54 if(WaitUpdate(vrpnobjects.at(vrpnobjects.size()-1),100000000)) {
55 SendObjects();
56 }
57 dataSocketTimeout=TIME_NONBLOCK;
58 } else {
59 dataSocketTimeout=100000000;
60 }
61
62 ssize_t rcv=dataSocket->RecvMessage(msg,sizeof(msg),dataSocketTimeout,NULL,NULL,&src_id);
63 if(rcv>0) {
64 int16_t id;
65 string object=string(msg,rcv-sizeof(id));
66 memcpy(&id,&msg[rcv-sizeof(id)],sizeof(id));
67 dataSocket->HostToNetwork((char*)&id,sizeof(id));
68 Printf("src id %i\n",src_id);
69 if(id>0) { //add object
70 if(src_id==vrpnobjects_list.size()) {
71 vector<VrpnObject*> vect;
72 vrpnobjects_list.push_back(vect);
73 //Printf("add vect %i\n",src_id);
74 }
75 //assume we receive it in the good order
76 if(id==vrpnobjects_list.at(src_id).size()+1) {//id 0 is not used as we use positive and negative id for add/remove
77 Printf("adding object %s with id %i from client %i\n",object.c_str(),id,src_id);
78 VrpnObject* vrpnobject = new VrpnObject(object,vrpnclient->GetTabWidget());
79 vrpnobjects.push_back(vrpnobject);
80 vrpnobjects_list.at(src_id).push_back(vrpnobject);
81 }else {
82 Err("adding object %s failed, expected id %i, got %i\n",object.c_str(),vrpnobjects_list.at(src_id).size()+1,id);
83 }
84 } else { //remove object
85 for (vector<VrpnObject*>::iterator it = vrpnobjects.begin();it < vrpnobjects.end(); it++) {
86 if ((*it)->ObjectName() == object) {
87 Printf("removing object %s with id %i from client %i\n",object.c_str(),-id,src_id);
88 auto iteratorEnd=remove(vrpnobjects_list.at(src_id).begin(), vrpnobjects_list.at(src_id).end(),(*it));
89 vrpnobjects_list.at(src_id).erase(iteratorEnd, vrpnobjects_list.at(src_id).end());
90 if(vrpnobjects_list.at(src_id).size()==0) {
91 Printf("not removing empty vrpnobjects_list (need code rewrite\n");
92 //le src_id va continuer a augmenter cote udtsocket et la comparaison if(src_id==vrpnobjects_list.size()) {
93 //ne va pas marcher; il faut identifier autrement les src
94 //auto iteratorEnd=remove(vrpnobjects_list.begin(), vrpnobjects_list.end(),vrpnobjects_list.at(src_id));
95 //vrpnobjects_list.erase(iteratorEnd, vrpnobjects_list.end());
96 }
97 delete (*it);
98 vrpnobjects.erase(it);
99 break;
100 }
101 }
102
103 }
104 }
105 }
106}
107
108void VrpnLite::SendObjects(void) const{
109
110 int16_t position[3];
111 int16_t quaternion[4];
112 Time time;
113
114 //for (vector<vector<VrpnObject*>>::const_iterator connection= vrpnobjects_list.begin();connection < vrpnobjects_list.end(); connection++) {
115 for (int i=0;i<vrpnobjects_list.size();i++) {
116 char datas[vrpnobjects_list.at(i).size()*(sizeof(position)+sizeof(quaternion))+ sizeof(time)];
117 char *datasPtr=datas;
118 for (vector<VrpnObject*>::const_iterator it = vrpnobjects_list.at(i).begin();it < vrpnobjects_list.at(i).end(); it++) {
119 Vector3Df objectPosition;
120 Quaternion objectQuaternion;
121 const VrpnObject* vrpnobject=*it;
122
123 vrpnobject->GetPosition(objectPosition);
124 vrpnobject->GetQuaternion(objectQuaternion);
125 time=vrpnobject->GetLastPacketTime();
126
127 position[0]=ConvertPosition(objectPosition.x);
128 position[1]=ConvertPosition(objectPosition.y);
129 position[2]=ConvertPosition(objectPosition.z);
130 quaternion[0]=ConvertQuaternion(objectQuaternion.q0);
131 quaternion[1]=ConvertQuaternion(objectQuaternion.q1);
132 quaternion[2]=ConvertQuaternion(objectQuaternion.q2);
133 quaternion[3]=ConvertQuaternion(objectQuaternion.q3);
134
135 for(int i=0;i<3;i++) dataSocket->HostToNetwork((char*)(&position[i]),sizeof(position[i]));
136 for(int i=0;i<4;i++) dataSocket->HostToNetwork((char*)(&quaternion[i]),sizeof(quaternion[i]));
137
138 memcpy(datasPtr,position, sizeof(position));
139 datasPtr+=sizeof(position);
140 memcpy(datasPtr,quaternion, sizeof(quaternion));
141 datasPtr+=sizeof(quaternion);
142 }
143 dataSocket->HostToNetwork((char*)(&time),sizeof(Time));
144 memcpy(datasPtr,&time, sizeof(time));//only one time for all VrpnObject; suppose it is the same!
145 dataSocket->SendMessage(datas,sizeof(datas),i);
146 }
147}
148
149int16_t VrpnLite::ConvertQuaternion(float value) const{
150 int16_t tmp;
151 tmp=value*32767.;
152 if(value<-1) {
153 tmp=-32767;
154 Warn("position value is %f, saturating it to %i\n",value,tmp);
155 }
156 if(value>1) {
157 tmp=32767;
158 Warn("position value is %f, saturating it to %i\n",value,tmp);
159 }
160 return tmp;
161}
162
163int16_t VrpnLite::ConvertPosition(float value) const{
164 int16_t tmp;
165 tmp=value*1000;
166 if(value<-32.768) {
167 tmp=-32768;
168 Warn("position value is %f, saturating it to %i\n",value,tmp);
169 }
170 if(value>32.767) {
171 tmp=32767;
172 Warn("position value is %f, saturating it to %i\n",value,tmp);
173 }
174 return tmp;
175}
Note: See TracBrowser for help on using the repository browser.