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

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

sensoractuator

File size: 6.9 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,std::string address,uint16_t us_period)
44{
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,SerialPort* serialport,uint16_t us_period)
63{
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{
84 if(!UseXbee())
85 {
86 //on fait une copie car le delete touche a xbee_objects_copy via RemoveTrackable
87 vector<VrpnObject*> trackables_copy=trackables;
88 for(unsigned int i=0;i<trackables_copy.size();i++) delete trackables_copy.at(i);
89 //trackables.clear();
90 }
91 else
92 {
93 //on fait une copie car le delete touche a xbee_objects_copy via RemoveTrackable
94 vector<xbee_object> xbee_objects_copy=xbee_objects;
95 for(unsigned int i=0;i<xbee_objects_copy.size();i++) delete xbee_objects_copy.at(i).vrpnobject->self;
96 }
97
98 delete main_tab;
99
100 if(!UseXbee())
101 {
102 //it will automatically delete connection
103 connection->removeReference();
104 }
105}
106
107void VrpnClient_impl::ComputeRotations(Vector3D& point)
108{
109 rotation_1->ComputeRotation(point);
110 rotation_2->ComputeRotation(point);
111}
112
113void VrpnClient_impl::ComputeRotations(Quaternion& quat)
114{
115 rotation_1->ComputeRotation(quat);
116 rotation_2->ComputeRotation(quat);
117}
118
119void VrpnClient_impl::AddTrackable(VrpnObject* obj)
120{
121 mutex->GetMutex();
122 trackables.push_back(obj);
123 mutex->ReleaseMutex();
124}
125
126void VrpnClient_impl::RemoveTrackable(VrpnObject* obj)
127{
128 mutex->GetMutex();
129 for(vector<VrpnObject*>::iterator it=trackables.begin() ; it < trackables.end(); it++ )
130 {
131 if(*it==obj)
132 {
133 trackables.erase (it);
134 break;
135 }
136 }
137 mutex->ReleaseMutex();
138}
139
140void VrpnClient_impl::AddTrackable(VrpnObject_impl* obj,uint8_t id)
141{
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
150void VrpnClient_impl::RemoveTrackable(VrpnObject_impl* obj)
151{
152 mutex->GetMutex();
153 for(vector<xbee_object>::iterator it=xbee_objects.begin() ; it < xbee_objects.end(); it++ )
154 {
155 if((*it).vrpnobject==obj)
156 {
157 xbee_objects.erase (it);
158 break;
159 }
160 }
161 mutex->ReleaseMutex();
162}
163
164bool VrpnClient_impl::UseXbee(void)
165{
166 if(connection==NULL)
167 {
168 return true;
169 }
170 else
171 {
172 return false;
173 }
174}
175
176void VrpnClient_impl::Run(void)
177{
178 while(!self->ToBeStopped())
179 {
180 if(UseXbee())
181 {
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)) read += serialport->Read(&response[read],sizeof(response)-read);
187 //int temps=(float)self->GetTime()/(1000*1000);
188//self->Printf("%i %i %i\n",temps-last,temps,last);
189//last=temps;
190 if(read<0)
191 {
192 //self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
193 }
194 else if (read != sizeof(response))
195 {
196 self->Warn("erreur rt_dev_read %i/%i\n",read,sizeof(response));
197 }
198 else
199 {
200 //for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
201 //printf("\n");
202 uint8_t checksum=0;
203 for(ssize_t i=3;i<read;i++) checksum+=response[i];
204 if(checksum!=255)
205 {
206 self->Err("checksum error\n");
207 }
208 else
209 {
210 vrpn_TRACKERCB t;
211 float pos[3];
212 float quat[4];
213 uint8_t id=response[8];
214
215 mutex->GetMutex();
216 if(id<xbee_objects.size())
217 {
218 memcpy(pos,&response[9],sizeof(pos));
219 memcpy(quat,&response[9]+sizeof(pos),sizeof(quat));
220 for(int i=0;i<3;i++) t.pos[i]=pos[i];
221 for(int i=0;i<4;i++) t.quat[i]=quat[i];
222 if(fabs(pos[0]>10) || fabs(pos[1]>10) || fabs(pos[2]>10))
223 {
224 printf("prob pos %f %f %f\n",pos[0],pos[1],pos[2]);
225 }
226 else
227 {
228 //self->Printf("%i %f %f %f %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
229 VrpnObject_impl::handle_pos(xbee_objects.at(id).vrpnobject,t);
230 }
231 }
232 mutex->ReleaseMutex();
233 }
234 }
235 }
236 else
237 {
238 connection->mainloop();
239 mutex->GetMutex();
240 for(unsigned int i=0;i<trackables.size();i++) trackables.at(i)->mainloop();
241 mutex->ReleaseMutex();
242
243 self->SleepUS(us_period);
244 }
245 }
246}
Note: See TracBrowser for help on using the repository browser.