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

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

changement post/pre rotation

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 <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",OneAxisRotation::PreRotation);
59 rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
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",OneAxisRotation::PreRotation);
79 rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
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.