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

Last change on this file since 25 was 15, checked in by Bayard Gildas, 9 years ago

sources reformatted with flair-format-dir script

File size: 6.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 <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, uint16_t us_period) {
45 this->us_period = us_period;
46 this->self = self;
47 serialport = NULL;
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 while (!self->ToBeStopped()) {
165 if (UseXbee()) {
166 ssize_t read = 0;
167 uint8_t response[38] = {0};
168
169 read = serialport->Read(response, sizeof(response));
170 if (read > 0 && read != sizeof(response))
171 read += serialport->Read(&response[read], sizeof(response) - read);
172 // int temps=(float)self->GetTime()/(1000*1000);
173 // self->Printf("%i %i %i\n",temps-last,temps,last);
174 // last=temps;
175 if (read < 0) {
176 // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
177 } else if (read != sizeof(response)) {
178 self->Warn("erreur rt_dev_read %i/%i\n", read, sizeof(response));
179 } else {
180 // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
181 // printf("\n");
182 uint8_t checksum = 0;
183 for (ssize_t i = 3; i < read; i++)
184 checksum += response[i];
185 if (checksum != 255) {
186 self->Err("checksum error\n");
187 } else {
188 vrpn_TRACKERCB t;
189 float pos[3];
190 float quat[4];
191 uint8_t id = response[8];
192
193 mutex->GetMutex();
194 if (id < xbee_objects.size()) {
195 memcpy(pos, &response[9], sizeof(pos));
196 memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
197 for (int i = 0; i < 3; i++)
198 t.pos[i] = pos[i];
199 for (int i = 0; i < 4; i++)
200 t.quat[i] = quat[i];
201 if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
202 printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
203 } else {
204 // self->Printf("%i %f %f %f
205 // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
206 VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject, t);
207 }
208 }
209 mutex->ReleaseMutex();
210 }
211 }
212 } else {
213 connection->mainloop();
214 mutex->GetMutex();
215 for (unsigned int i = 0; i < trackables.size(); i++)
216 trackables.at(i)->mainloop();
217 mutex->ReleaseMutex();
218
219 self->SleepUS(us_period);
220 }
221 }
222}
Note: See TracBrowser for help on using the repository browser.