[3] | 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 |
|
---|
| 37 | using std::string;
|
---|
| 38 | using std::vector;
|
---|
| 39 | using namespace flair::core;
|
---|
| 40 | using namespace flair::gui;
|
---|
| 41 | using namespace flair::sensor;
|
---|
| 42 |
|
---|
| 43 | VrpnClient_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 |
|
---|
| 62 | VrpnClient_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 |
|
---|
| 82 | VrpnClient_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 |
|
---|
| 107 | void VrpnClient_impl::ComputeRotations(Vector3D& point)
|
---|
| 108 | {
|
---|
| 109 | rotation_1->ComputeRotation(point);
|
---|
| 110 | rotation_2->ComputeRotation(point);
|
---|
| 111 | }
|
---|
| 112 |
|
---|
| 113 | void VrpnClient_impl::ComputeRotations(Quaternion& quat)
|
---|
| 114 | {
|
---|
| 115 | rotation_1->ComputeRotation(quat);
|
---|
| 116 | rotation_2->ComputeRotation(quat);
|
---|
| 117 | }
|
---|
| 118 |
|
---|
| 119 | void VrpnClient_impl::AddTrackable(VrpnObject* obj)
|
---|
| 120 | {
|
---|
| 121 | mutex->GetMutex();
|
---|
| 122 | trackables.push_back(obj);
|
---|
| 123 | mutex->ReleaseMutex();
|
---|
| 124 | }
|
---|
| 125 |
|
---|
| 126 | void 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 |
|
---|
| 140 | void 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 |
|
---|
| 150 | void 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 |
|
---|
| 164 | bool VrpnClient_impl::UseXbee(void)
|
---|
| 165 | {
|
---|
| 166 | if(connection==NULL)
|
---|
| 167 | {
|
---|
| 168 | return true;
|
---|
| 169 | }
|
---|
| 170 | else
|
---|
| 171 | {
|
---|
| 172 | return false;
|
---|
| 173 | }
|
---|
| 174 | }
|
---|
| 175 |
|
---|
| 176 | void 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 | }
|
---|