source: flair-src/branches/sanscv/lib/FlairSensorActuator/src/VrpnClient_impl.cpp@ 452

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

removing opencv dependency
File size: 9.4 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 <UdpSocket.h>
33#include <math.h>
34
35using std::string;
36using std::vector;
37using namespace flair::core;
38using namespace flair::gui;
39using namespace flair::sensor;
40
41VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
42 std::string address) {
43 this->self = self;
44 this->address = address;
45 isConnected=false;
46 connectionType=VrpnClient::Vrpn;
47
48 connection = vrpn_get_connection_by_name(address.c_str());
49
50 CommonConstructor(name);
51
52 Printf("Connecting to VRPN server on %s\n",address.c_str());
53}
54
55VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
56 uint16_t port) {
57 this->self = self;
58 isConnected=false;
59 connectionType=VrpnClient::VrpnLite;
60
61 dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", port);
62
63 CommonConstructor(name);
64
65 Printf("Connecting to VRPN-lite server on port %i\n",port);
66}
67
68VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
69 SerialPort *serialport, uint16_t us_period) {
70 this->us_period = us_period;
71 this->self = self;
72 this->serialport = serialport;
73 connectionType=VrpnClient::Xbee;
74
75 serialport->SetBaudrate(111111);
76 serialport->SetRxTimeout(us_period * 1000);
77
78 CommonConstructor(name);
79
80 Printf("Connecting to VRPN server through xbee on %s\n",serialport->ObjectName().c_str());
81}
82
83void VrpnClient_impl::CommonConstructor(std::string name) {
84 mutex = new Mutex(self, name);
85
86 // station sol
87 main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
88 tab = new TabWidget(main_tab->NewRow(), name);
89 setup_tab = new Tab(tab, "Reglages");
90
91 rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
92 rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
93}
94
95VrpnClient_impl::~VrpnClient_impl() {
96 if (connectionType==VrpnClient::Vrpn) {
97 // on fait une copie car le delete touche a trackables_copy via
98 // RemoveTrackable
99 vector<VrpnObject_impl *> trackables_copy = trackables;
100 for (unsigned int i = 0; i < trackables_copy.size(); i++)
101 delete trackables_copy.at(i)->self;
102 // trackables.clear();
103 } else if(connectionType==VrpnClient::Xbee || connectionType==VrpnClient::VrpnLite) {
104 // on fait une copie car le delete touche a xbee_objects_copy via
105 // RemoveTrackable
106 vector<liteObject_t> liteObjects_copy = liteObjects;
107 for (unsigned int i = 0; i < liteObjects_copy.size(); i++)
108 delete liteObjects_copy.at(i).vrpnobject->self;
109 }
110
111 delete main_tab;
112
113 if (connectionType==VrpnClient::Vrpn) {
114 // it will automatically delete connection
115 connection->removeReference();
116 }
117}
118
119void VrpnClient_impl::ComputeRotations(Vector3Df &point) {
120 rotation_1->ComputeRotation(point);
121 rotation_2->ComputeRotation(point);
122}
123
124void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
125 rotation_1->ComputeRotation(quat);
126 rotation_2->ComputeRotation(quat);
127}
128
129void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
130 if (connectionType==VrpnClient::Vrpn) {
131 mutex->GetMutex();
132 trackables.push_back(obj);
133 mutex->ReleaseMutex();
134 } else {
135 self->Warn("AddTrackable called but not in vrpn mode\n");
136 }
137}
138
139void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
140 if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
141 liteObject_t tmp;
142 tmp.vrpnobject = obj;
143 tmp.id = id;
144 mutex->GetMutex();
145 liteObjects.push_back(tmp);
146 mutex->ReleaseMutex();
147 } else {
148 self->Warn("AddTrackable called but not in vrpnlite nor in xbee mode\n");
149 }
150}
151
152void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
153 mutex->GetMutex();
154 if (connectionType==VrpnClient::Vrpn) {
155 for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
156 it < trackables.end(); it++) {
157 if (*it == obj) {
158 trackables.erase(it);
159 break;
160 }
161 }
162 }
163 if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
164 for (vector<liteObject_t>::iterator it = liteObjects.begin();
165 it < liteObjects.end(); it++) {
166 if ((*it).vrpnobject == obj) {
167 liteObjects.erase(it);
168 break;
169 }
170 }
171 }
172 mutex->ReleaseMutex();
173}
174
175void VrpnClient_impl::Run(void) {
176 struct timeval timeout;
177 timeout.tv_sec=0;
178 timeout.tv_usec=100000;
179
180 while (!self->ToBeStopped()) {
181 if (connectionType==VrpnClient::Xbee) {
182 ssize_t read = 0;
183 uint8_t response[38] = {0};
184
185 read = serialport->Read(response, sizeof(response));
186 if (read > 0 && read != sizeof(response))
187 read += serialport->Read(&response[read], sizeof(response) - read);
188 // int temps=(float)self->GetTime()/(1000*1000);
189 // self->Printf("%i %i %i\n",temps-last,temps,last);
190 // last=temps;
191 if (read < 0) {
192 // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
193 } else if (read != sizeof(response)) {
194 self->Warn("serial read error %i/%i\n", read, sizeof(response));
195 } else {
196 // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
197 // printf("\n");
198 uint8_t checksum = 0;
199 for (ssize_t i = 3; i < read; i++)
200 checksum += response[i];
201 if (checksum != 255) {
202 self->Err("checksum error\n");
203 } else {
204 vrpn_TRACKERCB t;
205 float pos[3];
206 float quat[4];
207 uint8_t id = response[8];
208
209 mutex->GetMutex();
210 if (id < liteObjects.size()) {
211 memcpy(pos, &response[9], sizeof(pos));
212 memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
213 for (int i = 0; i < 3; i++)
214 t.pos[i] = pos[i];
215 for (int i = 0; i < 4; i++)
216 t.quat[i] = quat[i];
217 if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
218 Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
219 } else {
220 // self->Printf("%i %f %f %f
221 // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
222 VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
223 }
224 }
225 mutex->ReleaseMutex();
226 }
227 }
228 } else if(connectionType==VrpnClient::Vrpn) {
229 if(connection->connected()==vrpn_true && !isConnected) {
230 isConnected=true;
231 Printf("VRPN connected to %s\n",address.c_str());
232 }
233 if(connection->connected()==vrpn_false && isConnected) {
234 isConnected=false;
235 Printf("VRPN disconnected to %s\n",address.c_str());
236 }
237 //timeout in mainloop is not well handled if not connected...
238 if(isConnected) {
239 //this is when trackables callbacks are called:
240 if(connection->mainloop(&timeout)!=0) {
241 self->Warn("connection dropped from\n",address.c_str());
242 }
243 //printf("%lld\n",GetTime()/(1000*1000));
244 mutex->GetMutex();
245 for (unsigned int i = 0; i < trackables.size(); i++)
246 trackables.at(i)->tracker->mainloop();
247 mutex->ReleaseMutex();
248 } else {
249 connection->mainloop();
250 self->SleepMS(10);
251 }
252 }else if(connectionType==VrpnClient::VrpnLite) {
253 vrpn_TRACKERCB t;
254 float pos[3];
255 float quat[4];
256 Time time;
257 uint8_t id;
258 char datas[sizeof(id) + sizeof(pos)+sizeof(quat)+ sizeof(time)];
259 int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
260 if(rcv!=sizeof(datas)) continue;
261 id = datas[0];
262 memcpy(pos, datas+sizeof(id), sizeof(pos));
263 memcpy(quat, datas +sizeof(id)+ sizeof(pos), sizeof(quat));
264 memcpy(&time, datas+sizeof(id) + sizeof(pos)+sizeof(quat), sizeof(time));
265
266 for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
267 for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
268 dataSocket->NetworkToHost((char*)(&time),sizeof(time));
269
270 mutex->GetMutex();
271 if (id < liteObjects.size()) {
272 for (int i = 0; i < 3; i++) t.pos[i] = pos[i];
273 // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
274 // flair::core::Quaternion
275 t.quat[0] = quat[1];
276 t.quat[1] = quat[2];
277 t.quat[2] = quat[3];
278 t.quat[3] = quat[0];
279 t.msg_time.tv_sec=time/((Time)1000000000);
280 t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
281 //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
282 VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
283 }
284 mutex->ReleaseMutex();
285 }
286 }
287}
Note: See TracBrowser for help on using the repository browser.