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

Last change on this file since 294 was 294, checked in by Sanahuja Guillaume, 3 years ago

vrpn modifs

File size: 7.3 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 this->address = address;
44 serialport = NULL;
45 isConnected=false;
46
47 mutex = new Mutex(self, name);
48
49 connection = vrpn_get_connection_by_name(address.c_str());
50
51 // station sol
52 main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
53 tab = new TabWidget(main_tab->NewRow(), name);
54 setup_tab = new Tab(tab, "Reglages");
55
56 rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
57 rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
58
59 Printf("Connecting to VRPN server on %s\n",address.c_str());
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 Printf("Connecting to VRPN server on %s\n",serialport->ObjectName().c_str());
82}
83
84VrpnClient_impl::~VrpnClient_impl() {
85 if (!UseXbee()) {
86 // on fait une copie car le delete touche a trackables_copy via
87 // RemoveTrackable
88 vector<VrpnObject_impl *> trackables_copy = trackables;
89 for (unsigned int i = 0; i < trackables_copy.size(); i++)
90 delete trackables_copy.at(i)->self;
91 // trackables.clear();
92 } else {
93 // on fait une copie car le delete touche a xbee_objects_copy via
94 // RemoveTrackable
95 vector<xbee_object> xbee_objects_copy = xbee_objects;
96 for (unsigned int i = 0; i < xbee_objects_copy.size(); i++)
97 delete xbee_objects_copy.at(i).vrpnobject->self;
98 }
99
100 delete main_tab;
101
102 if (!UseXbee()) {
103 // it will automatically delete connection
104 connection->removeReference();
105 }
106}
107
108void VrpnClient_impl::ComputeRotations(Vector3Df &point) {
109 rotation_1->ComputeRotation(point);
110 rotation_2->ComputeRotation(point);
111}
112
113void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
114 rotation_1->ComputeRotation(quat);
115 rotation_2->ComputeRotation(quat);
116}
117
118void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
119 mutex->GetMutex();
120 trackables.push_back(obj);
121 mutex->ReleaseMutex();
122}
123
124void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
125 mutex->GetMutex();
126 for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
127 it < trackables.end(); it++) {
128 if (*it == obj) {
129 trackables.erase(it);
130 break;
131 }
132 }
133 for (vector<xbee_object>::iterator it = xbee_objects.begin();
134 it < xbee_objects.end(); it++) {
135 if ((*it).vrpnobject == obj) {
136 xbee_objects.erase(it);
137 break;
138 }
139 }
140 mutex->ReleaseMutex();
141}
142
143void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
144 xbee_object tmp;
145 tmp.vrpnobject = obj;
146 tmp.id = id;
147 mutex->GetMutex();
148 xbee_objects.push_back(tmp);
149 mutex->ReleaseMutex();
150}
151
152bool VrpnClient_impl::UseXbee(void) {
153 if (connection == NULL) {
154 return true;
155 } else {
156 return false;
157 }
158}
159
160void VrpnClient_impl::Run(void) {
161 struct timeval timeout;
162 timeout.tv_sec=0;
163 timeout.tv_usec=100000;
164
165 while (!self->ToBeStopped()) {
166 if (UseXbee()) {
167 ssize_t read = 0;
168 uint8_t response[38] = {0};
169
170 read = serialport->Read(response, sizeof(response));
171 if (read > 0 && read != sizeof(response))
172 read += serialport->Read(&response[read], sizeof(response) - read);
173 // int temps=(float)self->GetTime()/(1000*1000);
174 // self->Printf("%i %i %i\n",temps-last,temps,last);
175 // last=temps;
176 if (read < 0) {
177 // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
178 } else if (read != sizeof(response)) {
179 self->Warn("serial read error %i/%i\n", read, sizeof(response));
180 } else {
181 // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
182 // printf("\n");
183 uint8_t checksum = 0;
184 for (ssize_t i = 3; i < read; i++)
185 checksum += response[i];
186 if (checksum != 255) {
187 self->Err("checksum error\n");
188 } else {
189 vrpn_TRACKERCB t;
190 float pos[3];
191 float quat[4];
192 uint8_t id = response[8];
193
194 mutex->GetMutex();
195 if (id < xbee_objects.size()) {
196 memcpy(pos, &response[9], sizeof(pos));
197 memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
198 for (int i = 0; i < 3; i++)
199 t.pos[i] = pos[i];
200 for (int i = 0; i < 4; i++)
201 t.quat[i] = quat[i];
202 if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
203 Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
204 } else {
205 // self->Printf("%i %f %f %f
206 // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
207 VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject, t);
208 }
209 }
210 mutex->ReleaseMutex();
211 }
212 }
213 } else {//!UseXbee()
214 if(connection->connected()==vrpn_true && !isConnected) {
215 isConnected=true;
216 Printf("VRPN connected to %s\n",address.c_str());
217 }
218 if(connection->connected()==vrpn_false && isConnected) {
219 isConnected=false;
220 Printf("VRPN disconnected to %s\n",address.c_str());
221 }
222 //timeout in mainloop is not well handled if not connected...
223 if(isConnected) {
224 //this is when trackables callbacks are called:
225 if(connection->mainloop(&timeout)!=0) {
226 self->Warn("connection dropped from\n",address.c_str());
227 }
228 //printf("%lld\n",GetTime()/(1000*1000));
229 mutex->GetMutex();
230 for (unsigned int i = 0; i < trackables.size(); i++)
231 trackables.at(i)->tracker->mainloop();
232 mutex->ReleaseMutex();
233 } else {
234 connection->mainloop();
235 self->SleepMS(10);
236 }
237 }
238 }
239}
Note: See TracBrowser for help on using the repository browser.