source: flair-src/trunk/lib/FlairSensorActuator/src/VrpnClient_impl.cpp@ 331

Last change on this file since 331 was 331, checked in by Sanahuja Guillaume, 5 years ago

resolve aligment trap bug
File size: 10.8 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5// created: 2013/04/03
6// filename: VrpnClient_impl.cpp
7//
8// author: César Richard, Guillaume Sanahuja
9// Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11// version: $Id: $
12//
13// purpose: objet se connectant au serveur vrpn
14//
15//
16/*********************************************************************/
17
18#include "VrpnClient.h"
19#include "VrpnClient_impl.h"
20#include "VrpnObject.h"
21#include "VrpnObject_impl.h"
22#include <SerialPort.h>
23#include <vrpn_Connection.h>
24#include <FrameworkManager.h>
25#include <TabWidget.h>
26#include <Tab.h>
27#include <GridLayout.h>
28#include <OneAxisRotation.h>
29#include <Vector3D.h>
30#include <Quaternion.h>
31#include <Mutex.h>
32#include <UdpSocket.h>
33#include <math.h>
34
35using std::string;
36using std::vector;
37using namespace flair::core;
38using namespace flair::gui;
39using namespace flair::sensor;
40
41VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
42 std::string address,flair::sensor::VrpnClient::ConnectionType_t connectionType) {
43 this->self = self;
44 this->address = address;
45 this->connectionType = connectionType;
46 isConnected=false;
47
48 if(connectionType==VrpnClient::Vrpn) {
49 connection = vrpn_get_connection_by_name(address.c_str());
50 Printf("Connecting to VRPN server on %s\n",address.c_str());
51 } else if(connectionType==VrpnClient::VrpnLite) {
52 dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", address);
53 Printf("Connecting to VRPN-lite server on %s\n",address.c_str());
54 } else {
55 self->Err("Bad connection type, try using naother constructor\n");
56 }
57
58 CommonConstructor(name);
59}
60
61
62
63VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
64 SerialPort *serialport, uint16_t us_period) {
65 this->us_period = us_period;
66 this->self = self;
67 this->serialport = serialport;
68 connectionType=VrpnClient::Xbee;
69
70 serialport->SetBaudrate(111111);
71 serialport->SetRxTimeout(us_period * 1000);
72
73 CommonConstructor(name);
74
75 Printf("Connecting to VRPN server through xbee on %s\n",serialport->ObjectName().c_str());
76}
77
78void VrpnClient_impl::CommonConstructor(std::string name) {
79 mutex = new Mutex(self, name);
80
81 // station sol
82 main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
83 tab = new TabWidget(main_tab->NewRow(), name);
84 setup_tab = new Tab(tab, "Reglages");
85
86 rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
87 rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
88}
89
90VrpnClient_impl::~VrpnClient_impl() {
91 if (connectionType==VrpnClient::Vrpn) {
92 // on fait une copie car le delete touche a trackables_copy via
93 // RemoveTrackable
94 vector<VrpnObject_impl *> trackables_copy = trackables;
95 for (unsigned int i = 0; i < trackables_copy.size(); i++)
96 delete trackables_copy.at(i)->self;
97 // trackables.clear();
98 } else if(connectionType==VrpnClient::Xbee || connectionType==VrpnClient::VrpnLite) {
99 // on fait une copie car le delete touche a xbee_objects_copy via
100 // RemoveTrackable
101 vector<liteObject_t> liteObjects_copy = liteObjects;
102 for (unsigned int i = 0; i < liteObjects_copy.size(); i++)
103 delete liteObjects_copy.at(i).vrpnobject->self;
104 }
105
106 delete main_tab;
107
108 if (connectionType==VrpnClient::Vrpn) {
109 // it will automatically delete connection
110 connection->removeReference();
111 }
112}
113
114void VrpnClient_impl::ComputeRotations(Vector3Df &point) {
115 rotation_1->ComputeRotation(point);
116 rotation_2->ComputeRotation(point);
117}
118
119void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
120 rotation_1->ComputeRotation(quat);
121 rotation_2->ComputeRotation(quat);
122}
123
124void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
125 if (connectionType==VrpnClient::Vrpn) {
126 mutex->GetMutex();
127 trackables.push_back(obj);
128 mutex->ReleaseMutex();
129 } else if (connectionType==VrpnClient::VrpnLite) {
130 if(liteObjects.size()<0xffff) {
131 liteObject_t tmp;
132 tmp.vrpnobject = obj;
133 uint16_t id=liteObjects.size();
134 mutex->GetMutex();
135 liteObjects.push_back(tmp);
136 mutex->ReleaseMutex();
137 //Printf("%i %s\n",tmp.id,obj->self->ObjectName().c_str());
138
139 char char_array[obj->self->ObjectName().length() + sizeof(id)];
140 strcpy(char_array, obj->self->ObjectName().c_str());
141 dataSocket->HostToNetwork((char*)(&id),sizeof(id));
142 memcpy(&char_array[obj->self->ObjectName().length()],&id,sizeof(id));
143 dataSocket->SendMessage(char_array,obj->self->ObjectName().length() + sizeof(id));
144 }else {
145 self->Warn("too much trackables for vrpnlite connection, not adding %s\n",obj->self->ObjectName().c_str());
146 }
147 } else {
148 self->Warn("AddTrackable called but not in vrpn mode nor in vrpnlite mode\n");
149 }
150}
151
152void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
153 if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
154 liteObject_t tmp;
155 tmp.vrpnobject = obj;
156 tmp.id = id;
157 mutex->GetMutex();
158 liteObjects.push_back(tmp);
159 mutex->ReleaseMutex();
160 } else {
161 self->Warn("AddTrackable called but not in xbee mode\n");
162 }
163}
164
165void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
166 mutex->GetMutex();
167 if (connectionType==VrpnClient::Vrpn) {
168 for (vector<VrpnObject_impl *>::iterator it = trackables.begin();it < trackables.end(); it++) {
169 if (*it == obj) {
170 trackables.erase(it);
171 break;
172 }
173 }
174 }
175 if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
176 for (vector<liteObject_t>::iterator it = liteObjects.begin();it < liteObjects.end(); it++) {
177 if ((*it).vrpnobject == obj) {
178 liteObjects.erase(it);
179 break;
180 }
181 }
182 }
183 mutex->ReleaseMutex();
184}
185
186void VrpnClient_impl::Run(void) {
187 struct timeval timeout;
188 timeout.tv_sec=0;
189 timeout.tv_usec=100000;
190
191 while (!self->ToBeStopped()) {
192 if (connectionType==VrpnClient::Xbee) {
193 ssize_t read = 0;
194 uint8_t response[38] = {0};
195
196 read = serialport->Read(response, sizeof(response));
197 if (read > 0 && read != sizeof(response))
198 read += serialport->Read(&response[read], sizeof(response) - read);
199 // int temps=(float)self->GetTime()/(1000*1000);
200 // self->Printf("%i %i %i\n",temps-last,temps,last);
201 // last=temps;
202 if (read < 0) {
203 // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
204 } else if (read != sizeof(response)) {
205 self->Warn("serial read error %i/%i\n", read, sizeof(response));
206 } else {
207 // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
208 // printf("\n");
209 uint8_t checksum = 0;
210 for (ssize_t i = 3; i < read; i++)
211 checksum += response[i];
212 if (checksum != 255) {
213 self->Err("checksum error\n");
214 } else {
215 vrpn_TRACKERCB t;
216 float pos[3];
217 float quat[4];
218 uint8_t id = response[8];
219
220 mutex->GetMutex();
221 if (id < liteObjects.size()) {
222 memcpy(pos, &response[9], sizeof(pos));
223 memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
224 for (int i = 0; i < 3; i++)
225 t.pos[i] = pos[i];
226 for (int i = 0; i < 4; i++)
227 t.quat[i] = quat[i];
228 if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
229 Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
230 } else {
231 // self->Printf("%i %f %f %f
232 // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
233 VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
234 }
235 }
236 mutex->ReleaseMutex();
237 }
238 }
239 } else if(connectionType==VrpnClient::Vrpn) {
240 if(connection->connected()==vrpn_true && !isConnected) {
241 isConnected=true;
242 Printf("VRPN connected to %s\n",address.c_str());
243 }
244 if(connection->connected()==vrpn_false && isConnected) {
245 isConnected=false;
246 Printf("VRPN disconnected to %s\n",address.c_str());
247 }
248 //timeout in mainloop is not well handled if not connected...
249 if(isConnected) {
250 //this is when trackables callbacks are called:
251 if(connection->mainloop(&timeout)!=0) {
252 self->Warn("connection dropped from\n",address.c_str());
253 }
254 //printf("%lld\n",GetTime()/(1000*1000));
255 mutex->GetMutex();
256 for (unsigned int i = 0; i < trackables.size(); i++) {
257 // Printf("tracker %i\n",i);
258 trackables.at(i)->tracker->mainloop();
259 //Printf("tracker %i ok\n",i);
260 }
261 mutex->ReleaseMutex();
262 } else {
263 connection->mainloop();
264 self->SleepMS(10);
265 }
266 }else if(connectionType==VrpnClient::VrpnLite) {
267 mutex->GetMutex();
268
269 int16_t pos[3];
270 int16_t quat[4];
271 Time time;
272 char datas[liteObjects.size()*(sizeof(pos)+sizeof(quat))+ sizeof(time)];
273 char *datasPtr=datas;
274
275 int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
276 if(rcv!=sizeof(datas)) {
277 if(rcv>0) Printf("discarding message (size %i/%i)\n",rcv,sizeof(datas));
278 mutex->ReleaseMutex();
279 continue;
280 }
281
282 memcpy(&time, datasPtr+sizeof(datas)-sizeof(time), sizeof(time));
283 dataSocket->NetworkToHost((char*)(&time),sizeof(time));
284
285 for (vector<liteObject_t>::iterator it = liteObjects.begin();it < liteObjects.end(); it++) {
286 memcpy(pos, datasPtr, sizeof(pos));
287 datasPtr+=sizeof(pos);
288 memcpy(quat,datasPtr, sizeof(quat));
289 datasPtr+=sizeof(quat);
290
291 for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
292 for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
293
294 vrpn_TRACKERCB t;
295 for (int i = 0; i < 3; i++) t.pos[i] = ConvertPosition(pos[i]);
296 // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
297 // flair::core::Quaternion
298 t.quat[0] = ConvertQuaternion(quat[1]);
299 t.quat[1] = ConvertQuaternion(quat[2]);
300 t.quat[2] = ConvertQuaternion(quat[3]);
301 t.quat[3] = ConvertQuaternion(quat[0]);
302 t.msg_time.tv_sec=time/((Time)1000000000);
303 t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
304 //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
305 VrpnObject_impl::handle_pos((void*)(it->vrpnobject), t);
306 }
307
308 mutex->ReleaseMutex();
309 }
310 }
311}
312
313float VrpnClient_impl::ConvertPosition(int16_t value) const {
314 return (float)value/1000.;
315}
316
317float VrpnClient_impl::ConvertQuaternion(int16_t value) const {
318 return (float)value/32767.;
319}
Note: See TracBrowser for help on using the repository browser.