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, 6 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.