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

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

use less bandwidth in vprnlite

File size: 10.9 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      tmp.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() + 2];//id coded on 16bits
140      strcpy(char_array, obj->self->ObjectName().c_str()); 
141      uint16_t* idPtr=(uint16_t*)&char_array[obj->self->ObjectName().length()];
142      *idPtr=tmp.id;
143      dataSocket->HostToNetwork((char*)idPtr,sizeof(uint16_t));
144      dataSocket->SendMessage(char_array,obj->self->ObjectName().length() + 2);
145    }else {
146      self->Warn("too much trackables for vrpnlite connection, not adding %s\n",obj->self->ObjectName().c_str());
147    }
148  } else {
149    self->Warn("AddTrackable called but not in vrpn mode nor in vrpnlite mode\n");
150  }
151}
152
153void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
154  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
155    liteObject_t tmp;
156    tmp.vrpnobject = obj;
157    tmp.id = id;
158    mutex->GetMutex();
159    liteObjects.push_back(tmp);
160    mutex->ReleaseMutex();
161  } else {
162    self->Warn("AddTrackable called but not in xbee mode\n");
163  }
164}
165
166void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
167  mutex->GetMutex();
168  if (connectionType==VrpnClient::Vrpn) {
169    for (vector<VrpnObject_impl *>::iterator it = trackables.begin();it < trackables.end(); it++) {
170      if (*it == obj) {
171        trackables.erase(it);
172        break;
173      }
174    }
175  }
176  if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
177    for (vector<liteObject_t>::iterator it = liteObjects.begin();it < liteObjects.end(); it++) {
178      if ((*it).vrpnobject == obj) {
179        liteObjects.erase(it);
180        break;
181      }
182    }
183  }
184  mutex->ReleaseMutex();
185}
186
187void VrpnClient_impl::Run(void) {
188        struct timeval timeout;
189        timeout.tv_sec=0;
190        timeout.tv_usec=100000;
191                       
192  while (!self->ToBeStopped()) {
193    if (connectionType==VrpnClient::Xbee) {
194      ssize_t read = 0;
195      uint8_t response[38] = {0};
196
197      read = serialport->Read(response, sizeof(response));
198      if (read > 0 && read != sizeof(response))
199        read += serialport->Read(&response[read], sizeof(response) - read);
200      // int temps=(float)self->GetTime()/(1000*1000);
201      // self->Printf("%i %i %i\n",temps-last,temps,last);
202      // last=temps;
203      if (read < 0) {
204        // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
205      } else if (read != sizeof(response)) {
206        self->Warn("serial read error %i/%i\n", read, sizeof(response));
207      } else {
208        // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
209        // printf("\n");
210        uint8_t checksum = 0;
211        for (ssize_t i = 3; i < read; i++)
212          checksum += response[i];
213        if (checksum != 255) {
214          self->Err("checksum error\n");
215        } else {
216          vrpn_TRACKERCB t;
217          float pos[3];
218          float quat[4];
219          uint8_t id = response[8];
220
221          mutex->GetMutex();
222          if (id < liteObjects.size()) {
223            memcpy(pos, &response[9], sizeof(pos));
224            memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
225            for (int i = 0; i < 3; i++)
226              t.pos[i] = pos[i];
227            for (int i = 0; i < 4; i++)
228              t.quat[i] = quat[i];
229            if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
230              Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
231            } else {
232              // self->Printf("%i %f %f %f
233              // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
234              VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
235            }
236          }
237          mutex->ReleaseMutex();
238        }
239      }
240    } else if(connectionType==VrpnClient::Vrpn) {
241        if(connection->connected()==vrpn_true && !isConnected) {
242            isConnected=true;
243            Printf("VRPN connected to %s\n",address.c_str());
244        }
245        if(connection->connected()==vrpn_false && isConnected) {
246            isConnected=false;
247            Printf("VRPN disconnected to %s\n",address.c_str());
248        }
249        //timeout in mainloop is not well handled if not connected...
250        if(isConnected) {
251            //this is when trackables callbacks are called:
252            if(connection->mainloop(&timeout)!=0) {
253                self->Warn("connection dropped from\n",address.c_str());
254            }
255            //printf("%lld\n",GetTime()/(1000*1000));
256            mutex->GetMutex();
257            for (unsigned int i = 0; i < trackables.size(); i++) {
258             // Printf("tracker %i\n",i);
259              trackables.at(i)->tracker->mainloop();
260              //Printf("tracker %i ok\n",i);
261            }
262            mutex->ReleaseMutex();
263        } else {
264            connection->mainloop();
265            self->SleepMS(10);
266        }
267    }else if(connectionType==VrpnClient::VrpnLite) {
268      mutex->GetMutex();
269   
270      int16_t pos[3];
271      int16_t quat[4];
272      Time time;
273      char datas[liteObjects.size()*(sizeof(pos)+sizeof(quat))+ sizeof(time)];
274      char *datasPtr=datas;
275     
276      int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
277      if(rcv!=sizeof(datas)) {
278        if(rcv>0) Printf("discarding message (size %i/%i)\n",rcv,sizeof(datas));
279        mutex->ReleaseMutex();
280        continue; 
281      }
282     
283      memcpy(&time, datasPtr+sizeof(datas)-sizeof(time), sizeof(time));
284      dataSocket->NetworkToHost((char*)(&time),sizeof(time));
285     
286      for (vector<liteObject_t>::iterator it = liteObjects.begin();it < liteObjects.end(); it++) {
287        memcpy(pos, datasPtr, sizeof(pos));
288        datasPtr+=sizeof(pos);
289        memcpy(quat,datasPtr, sizeof(quat));
290        datasPtr+=sizeof(quat);
291       
292        for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
293        for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
294       
295        vrpn_TRACKERCB t;
296        for (int i = 0; i < 3; i++) t.pos[i] = ConvertPosition(pos[i]);
297        // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
298        // flair::core::Quaternion
299        t.quat[0] = ConvertQuaternion(quat[1]);
300        t.quat[1] = ConvertQuaternion(quat[2]);
301        t.quat[2] = ConvertQuaternion(quat[3]);
302        t.quat[3] = ConvertQuaternion(quat[0]);
303        t.msg_time.tv_sec=time/((Time)1000000000);
304        t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
305        //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
306        VrpnObject_impl::handle_pos((void*)(it->vrpnobject), t);
307      }
308     
309      mutex->ReleaseMutex();
310    }
311  }
312}
313
314float VrpnClient_impl::ConvertPosition(int16_t value) const {
315  return (float)value/1000.;
316}
317
318float VrpnClient_impl::ConvertQuaternion(int16_t value) const {
319  return (float)value/32767.;
320}
Note: See TracBrowser for help on using the repository browser.