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

Last change on this file since 4 was 3, checked in by Sanahuja Guillaume, 6 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.