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