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

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

vrpnbridge

File size: 7.1 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 <math.h>
33
34using std::string;
35using std::vector;
36using namespace flair::core;
37using namespace flair::gui;
38using namespace flair::sensor;
39
40VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
41                                 std::string address) {
42  this->self = self;
43  serialport = NULL;
44        isConnected=false;
45
46  mutex = new Mutex(self, name);
47
48  connection = vrpn_get_connection_by_name(address.c_str());
49
50  // station sol
51  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
52  tab = new TabWidget(main_tab->NewRow(), name);
53  setup_tab = new Tab(tab, "Reglages");
54
55  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
56  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
57}
58
59VrpnClient_impl::VrpnClient_impl(VrpnClient *self, std::string name,
60                                 SerialPort *serialport, uint16_t us_period) {
61  this->us_period = us_period;
62  this->self = self;
63  this->serialport = serialport;
64  connection = NULL;
65  mutex = new Mutex(self, name);
66
67  serialport->SetBaudrate(111111);
68  serialport->SetRxTimeout(us_period * 1000);
69
70  // station sol
71  main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
72  tab = new TabWidget(main_tab->NewRow(), name);
73  setup_tab = new Tab(tab, "Reglages");
74
75  rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
76  rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
77}
78
79VrpnClient_impl::~VrpnClient_impl() {
80  if (!UseXbee()) {
81    // on fait une copie car le delete touche a trackables_copy via
82    // RemoveTrackable
83    vector<VrpnObject_impl *> trackables_copy = trackables;
84    for (unsigned int i = 0; i < trackables_copy.size(); i++)
85      delete trackables_copy.at(i)->self;
86    // trackables.clear();
87  } else {
88    // on fait une copie car le delete touche a xbee_objects_copy via
89    // RemoveTrackable
90    vector<xbee_object> xbee_objects_copy = xbee_objects;
91    for (unsigned int i = 0; i < xbee_objects_copy.size(); i++)
92      delete xbee_objects_copy.at(i).vrpnobject->self;
93  }
94
95  delete main_tab;
96
97  if (!UseXbee()) {
98    // it will automatically delete connection
99    connection->removeReference();
100  }
101}
102
103void VrpnClient_impl::ComputeRotations(Vector3Df &point) {
104  rotation_1->ComputeRotation(point);
105  rotation_2->ComputeRotation(point);
106}
107
108void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
109  rotation_1->ComputeRotation(quat);
110  rotation_2->ComputeRotation(quat);
111}
112
113void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
114  mutex->GetMutex();
115  trackables.push_back(obj);
116  mutex->ReleaseMutex();
117}
118
119void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
120  mutex->GetMutex();
121  for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
122       it < trackables.end(); it++) {
123    if (*it == obj) {
124      trackables.erase(it);
125      break;
126    }
127  }
128  for (vector<xbee_object>::iterator it = xbee_objects.begin();
129       it < xbee_objects.end(); it++) {
130    if ((*it).vrpnobject == obj) {
131      xbee_objects.erase(it);
132      break;
133    }
134  }
135  mutex->ReleaseMutex();
136}
137
138void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
139  xbee_object tmp;
140  tmp.vrpnobject = obj;
141  tmp.id = id;
142  mutex->GetMutex();
143  xbee_objects.push_back(tmp);
144  mutex->ReleaseMutex();
145}
146
147bool VrpnClient_impl::UseXbee(void) {
148  if (connection == NULL) {
149    return true;
150  } else {
151    return false;
152  }
153}
154
155void VrpnClient_impl::Run(void) {
156        struct timeval timeout;
157        timeout.tv_sec=0;
158        timeout.tv_usec=100000;
159                       
160  while (!self->ToBeStopped()) {
161    if (UseXbee()) {
162      ssize_t read = 0;
163      uint8_t response[38] = {0};
164
165      read = serialport->Read(response, sizeof(response));
166      if (read > 0 && read != sizeof(response))
167        read += serialport->Read(&response[read], sizeof(response) - read);
168      // int temps=(float)self->GetTime()/(1000*1000);
169      // self->Printf("%i %i %i\n",temps-last,temps,last);
170      // last=temps;
171      if (read < 0) {
172        // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
173      } else if (read != sizeof(response)) {
174        self->Warn("erreur rt_dev_read %i/%i\n", read, sizeof(response));
175      } else {
176        // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
177        // printf("\n");
178        uint8_t checksum = 0;
179        for (ssize_t i = 3; i < read; i++)
180          checksum += response[i];
181        if (checksum != 255) {
182          self->Err("checksum error\n");
183        } else {
184          vrpn_TRACKERCB t;
185          float pos[3];
186          float quat[4];
187          uint8_t id = response[8];
188
189          mutex->GetMutex();
190          if (id < xbee_objects.size()) {
191            memcpy(pos, &response[9], sizeof(pos));
192            memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
193            for (int i = 0; i < 3; i++)
194              t.pos[i] = pos[i];
195            for (int i = 0; i < 4; i++)
196              t.quat[i] = quat[i];
197            if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
198              Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
199            } else {
200              // self->Printf("%i %f %f %f
201              // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
202              VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject, t);
203            }
204          }
205          mutex->ReleaseMutex();
206        }
207      }
208    } else {//!UseXbee()
209        if(connection->connected()==vrpn_true && !isConnected) {
210            isConnected=true;
211            Printf("VRPN connected\n");
212        }
213        if(connection->connected()==vrpn_false && isConnected) {
214            isConnected=false;
215            Printf("VRPN disconnected\n");
216        }
217        //timeout in mainloop is not well handled if not connected...
218        if(isConnected) {
219            //this is when trackables callbacks are called:
220            if(connection->mainloop(&timeout)!=0) {
221                self->Warn("connection dropped\n");
222            }
223            //printf("%lld\n",GetTime()/(1000*1000));
224            mutex->GetMutex();
225            for (unsigned int i = 0; i < trackables.size(); i++)
226                trackables.at(i)->tracker->mainloop();
227            mutex->ReleaseMutex();
228        } else {
229            connection->mainloop();
230            self->SleepMS(10);
231        }
232    }
233  }
234}
Note: See TracBrowser for help on using the repository browser.