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

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

vrpn: improved timeout

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