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

Last change on this file since 167 was 167, checked in by Sanahuja Guillaume, 4 years ago

modifs pour template vectors

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 trackables_copy via
85    // RemoveTrackable
86    vector<VrpnObject_impl *> trackables_copy = trackables;
87    for (unsigned int i = 0; i < trackables_copy.size(); i++)
88      delete trackables_copy.at(i)->self;
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(Vector3Df &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_impl *obj) {
117  mutex->GetMutex();
118  trackables.push_back(obj);
119  mutex->ReleaseMutex();
120}
121
122void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
123  mutex->GetMutex();
124  for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
125       it < trackables.end(); it++) {
126    if (*it == obj) {
127      trackables.erase(it);
128      break;
129    }
130  }
131  for (vector<xbee_object>::iterator it = xbee_objects.begin();
132       it < xbee_objects.end(); it++) {
133    if ((*it).vrpnobject == obj) {
134      xbee_objects.erase(it);
135      break;
136    }
137  }
138  mutex->ReleaseMutex();
139}
140
141void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
142  xbee_object tmp;
143  tmp.vrpnobject = obj;
144  tmp.id = id;
145  mutex->GetMutex();
146  xbee_objects.push_back(tmp);
147  mutex->ReleaseMutex();
148}
149
150bool VrpnClient_impl::UseXbee(void) {
151  if (connection == NULL) {
152    return true;
153  } else {
154    return false;
155  }
156}
157
158void VrpnClient_impl::Run(void) {
159        struct timeval timeout;
160        timeout.tv_sec=0;
161        timeout.tv_usec=100000;
162                       
163  while (!self->ToBeStopped()) {
164    if (UseXbee()) {
165      ssize_t read = 0;
166      uint8_t response[38] = {0};
167
168      read = serialport->Read(response, sizeof(response));
169      if (read > 0 && read != sizeof(response))
170        read += serialport->Read(&response[read], sizeof(response) - read);
171      // int temps=(float)self->GetTime()/(1000*1000);
172      // self->Printf("%i %i %i\n",temps-last,temps,last);
173      // last=temps;
174      if (read < 0) {
175        // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
176      } else if (read != sizeof(response)) {
177        self->Warn("erreur rt_dev_read %i/%i\n", read, sizeof(response));
178      } else {
179        // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
180        // printf("\n");
181        uint8_t checksum = 0;
182        for (ssize_t i = 3; i < read; i++)
183          checksum += response[i];
184        if (checksum != 255) {
185          self->Err("checksum error\n");
186        } else {
187          vrpn_TRACKERCB t;
188          float pos[3];
189          float quat[4];
190          uint8_t id = response[8];
191
192          mutex->GetMutex();
193          if (id < xbee_objects.size()) {
194            memcpy(pos, &response[9], sizeof(pos));
195            memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
196            for (int i = 0; i < 3; i++)
197              t.pos[i] = pos[i];
198            for (int i = 0; i < 4; i++)
199              t.quat[i] = quat[i];
200            if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
201              Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
202            } else {
203              // self->Printf("%i %f %f %f
204              // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
205              VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject, t);
206            }
207          }
208          mutex->ReleaseMutex();
209        }
210      }
211    } else {//!UseXbee()
212        if(connection->connected()==vrpn_true && !isConnected) {
213            isConnected=true;
214            Printf("VRPN connected\n");
215        }
216        if(connection->connected()==vrpn_false && isConnected) {
217            isConnected=false;
218            Printf("VRPN disconnected\n");
219        }
220        //timeout in mainloop is not well handled if not connected...
221        if(isConnected) {
222            //this is when trackables callbacks are called:
223            if(connection->mainloop(&timeout)!=0) {
224                self->Warn("connection dropped\n");
225            }
226            //printf("%lld\n",GetTime()/(1000*1000));
227            mutex->GetMutex();
228            for (unsigned int i = 0; i < trackables.size(); i++)
229                trackables.at(i)->tracker->mainloop();
230            mutex->ReleaseMutex();
231        } else {
232            connection->mainloop();
233            self->SleepMS(10);
234        }
235    }
236  }
237}
Note: See TracBrowser for help on using the repository browser.