[3] | 1 | // %flair:license{
|
---|
[15] | 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
[3] | 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>
|
---|
[309] | 32 | #include <UdpSocket.h>
|
---|
[3] | 33 | #include <math.h>
|
---|
| 34 |
|
---|
| 35 | using std::string;
|
---|
| 36 | using std::vector;
|
---|
| 37 | using namespace flair::core;
|
---|
| 38 | using namespace flair::gui;
|
---|
| 39 | using namespace flair::sensor;
|
---|
| 40 |
|
---|
[309] | 41 | VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
|
---|
[330] | 42 | std::string address,flair::sensor::VrpnClient::ConnectionType_t connectionType) {
|
---|
[15] | 43 | this->self = self;
|
---|
[294] | 44 | this->address = address;
|
---|
[330] | 45 | this->connectionType = connectionType;
|
---|
[309] | 46 | isConnected=false;
|
---|
[330] | 47 |
|
---|
| 48 | if(connectionType==VrpnClient::Vrpn) {
|
---|
| 49 | connection = vrpn_get_connection_by_name(address.c_str());
|
---|
| 50 | Printf("Connecting to VRPN server on %s\n",address.c_str());
|
---|
| 51 | } else if(connectionType==VrpnClient::VrpnLite) {
|
---|
| 52 | dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", address);
|
---|
| 53 | Printf("Connecting to VRPN-lite server on %s\n",address.c_str());
|
---|
| 54 | } else {
|
---|
| 55 | self->Err("Bad connection type, try using naother constructor\n");
|
---|
| 56 | }
|
---|
[3] | 57 |
|
---|
[309] | 58 | CommonConstructor(name);
|
---|
[3] | 59 | }
|
---|
| 60 |
|
---|
[309] | 61 |
|
---|
| 62 |
|
---|
| 63 | VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
|
---|
[15] | 64 | SerialPort *serialport, uint16_t us_period) {
|
---|
| 65 | this->us_period = us_period;
|
---|
| 66 | this->self = self;
|
---|
| 67 | this->serialport = serialport;
|
---|
[309] | 68 | connectionType=VrpnClient::Xbee;
|
---|
[3] | 69 |
|
---|
[15] | 70 | serialport->SetBaudrate(111111);
|
---|
| 71 | serialport->SetRxTimeout(us_period * 1000);
|
---|
[3] | 72 |
|
---|
[309] | 73 | CommonConstructor(name);
|
---|
| 74 |
|
---|
| 75 | Printf("Connecting to VRPN server through xbee on %s\n",serialport->ObjectName().c_str());
|
---|
| 76 | }
|
---|
| 77 |
|
---|
| 78 | void VrpnClient_impl::CommonConstructor(std::string name) {
|
---|
| 79 | mutex = new Mutex(self, name);
|
---|
| 80 |
|
---|
[15] | 81 | // station sol
|
---|
| 82 | main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
|
---|
| 83 | tab = new TabWidget(main_tab->NewRow(), name);
|
---|
| 84 | setup_tab = new Tab(tab, "Reglages");
|
---|
[3] | 85 |
|
---|
[187] | 86 | rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
|
---|
| 87 | rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
|
---|
[3] | 88 | }
|
---|
| 89 |
|
---|
[15] | 90 | VrpnClient_impl::~VrpnClient_impl() {
|
---|
[332] | 91 | if (connectionType==VrpnClient::Vrpn|| connectionType==VrpnClient::VrpnLite) {
|
---|
[140] | 92 | // on fait une copie car le delete touche a trackables_copy via
|
---|
[15] | 93 | // RemoveTrackable
|
---|
[140] | 94 | vector<VrpnObject_impl *> trackables_copy = trackables;
|
---|
[15] | 95 | for (unsigned int i = 0; i < trackables_copy.size(); i++)
|
---|
[140] | 96 | delete trackables_copy.at(i)->self;
|
---|
[15] | 97 | // trackables.clear();
|
---|
[332] | 98 | } else if(connectionType==VrpnClient::Xbee ) {
|
---|
[15] | 99 | // on fait une copie car le delete touche a xbee_objects_copy via
|
---|
| 100 | // RemoveTrackable
|
---|
[332] | 101 | vector<xbeeObject_t> xbeeObjects_copy = xbeeObjects;
|
---|
| 102 | for (unsigned int i = 0; i < xbeeObjects_copy.size(); i++)
|
---|
| 103 | delete xbeeObjects_copy.at(i).vrpnobject->self;
|
---|
[15] | 104 | }
|
---|
[3] | 105 |
|
---|
[15] | 106 | delete main_tab;
|
---|
[3] | 107 |
|
---|
[309] | 108 | if (connectionType==VrpnClient::Vrpn) {
|
---|
[15] | 109 | // it will automatically delete connection
|
---|
| 110 | connection->removeReference();
|
---|
| 111 | }
|
---|
[3] | 112 | }
|
---|
| 113 |
|
---|
[167] | 114 | void VrpnClient_impl::ComputeRotations(Vector3Df &point) {
|
---|
[15] | 115 | rotation_1->ComputeRotation(point);
|
---|
| 116 | rotation_2->ComputeRotation(point);
|
---|
[3] | 117 | }
|
---|
| 118 |
|
---|
[15] | 119 | void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
|
---|
| 120 | rotation_1->ComputeRotation(quat);
|
---|
| 121 | rotation_2->ComputeRotation(quat);
|
---|
[3] | 122 | }
|
---|
| 123 |
|
---|
[140] | 124 | void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
|
---|
[309] | 125 | if (connectionType==VrpnClient::Vrpn) {
|
---|
| 126 | mutex->GetMutex();
|
---|
| 127 | trackables.push_back(obj);
|
---|
| 128 | mutex->ReleaseMutex();
|
---|
[330] | 129 | } else if (connectionType==VrpnClient::VrpnLite) {
|
---|
[447] | 130 | if(trackables.size()<INT16_MAX) {
|
---|
| 131 | int16_t id=trackables.size()+1;//avoid 0; positive values for adding, negative for removing
|
---|
[330] | 132 | mutex->GetMutex();
|
---|
[332] | 133 | trackables.push_back(obj);
|
---|
[330] | 134 | mutex->ReleaseMutex();
|
---|
| 135 |
|
---|
[331] | 136 | char char_array[obj->self->ObjectName().length() + sizeof(id)];
|
---|
| 137 | strcpy(char_array, obj->self->ObjectName().c_str());
|
---|
| 138 | dataSocket->HostToNetwork((char*)(&id),sizeof(id));
|
---|
| 139 | memcpy(&char_array[obj->self->ObjectName().length()],&id,sizeof(id));
|
---|
| 140 | dataSocket->SendMessage(char_array,obj->self->ObjectName().length() + sizeof(id));
|
---|
[330] | 141 | }else {
|
---|
| 142 | self->Warn("too much trackables for vrpnlite connection, not adding %s\n",obj->self->ObjectName().c_str());
|
---|
| 143 | }
|
---|
[309] | 144 | } else {
|
---|
[330] | 145 | self->Warn("AddTrackable called but not in vrpn mode nor in vrpnlite mode\n");
|
---|
[309] | 146 | }
|
---|
[3] | 147 | }
|
---|
| 148 |
|
---|
[309] | 149 | void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
|
---|
[332] | 150 | if (connectionType==VrpnClient::Xbee) {
|
---|
| 151 | xbeeObject_t tmp;
|
---|
[309] | 152 | tmp.vrpnobject = obj;
|
---|
| 153 | tmp.id = id;
|
---|
| 154 | mutex->GetMutex();
|
---|
[332] | 155 | xbeeObjects.push_back(tmp);
|
---|
[309] | 156 | mutex->ReleaseMutex();
|
---|
| 157 | } else {
|
---|
[330] | 158 | self->Warn("AddTrackable called but not in xbee mode\n");
|
---|
[309] | 159 | }
|
---|
| 160 | }
|
---|
| 161 |
|
---|
[140] | 162 | void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
|
---|
[15] | 163 | mutex->GetMutex();
|
---|
[447] | 164 | //send remove notification to vrpnlite
|
---|
| 165 | if (connectionType==VrpnClient::VrpnLite) {
|
---|
| 166 | for (size_t i = 0; i < trackables.size(); i++) {
|
---|
| 167 | if(trackables.at(i)==obj) {
|
---|
| 168 | int16_t id=-(i+1);//avoid 0; positive values for adding, negative for removing
|
---|
| 169 |
|
---|
| 170 | char char_array[obj->self->ObjectName().length() + sizeof(id)];
|
---|
| 171 | strcpy(char_array, obj->self->ObjectName().c_str());
|
---|
| 172 | dataSocket->HostToNetwork((char*)(&id),sizeof(id));
|
---|
| 173 | memcpy(&char_array[obj->self->ObjectName().length()],&id,sizeof(id));
|
---|
| 174 | dataSocket->SendMessage(char_array,obj->self->ObjectName().length() + sizeof(id));
|
---|
| 175 | break;
|
---|
| 176 | }
|
---|
| 177 | }
|
---|
| 178 | }
|
---|
| 179 |
|
---|
| 180 | //update vector
|
---|
[332] | 181 | if (connectionType==VrpnClient::Vrpn || connectionType==VrpnClient::VrpnLite) {
|
---|
[330] | 182 | for (vector<VrpnObject_impl *>::iterator it = trackables.begin();it < trackables.end(); it++) {
|
---|
[309] | 183 | if (*it == obj) {
|
---|
| 184 | trackables.erase(it);
|
---|
| 185 | break;
|
---|
| 186 | }
|
---|
[3] | 187 | }
|
---|
[15] | 188 | }
|
---|
[332] | 189 | if ( connectionType==VrpnClient::Xbee) {
|
---|
| 190 | for (vector<xbeeObject_t>::iterator it = xbeeObjects.begin();it < xbeeObjects.end(); it++) {
|
---|
[309] | 191 | if ((*it).vrpnobject == obj) {
|
---|
[332] | 192 | xbeeObjects.erase(it);
|
---|
[309] | 193 | break;
|
---|
| 194 | }
|
---|
[140] | 195 | }
|
---|
| 196 | }
|
---|
[15] | 197 | mutex->ReleaseMutex();
|
---|
[3] | 198 | }
|
---|
| 199 |
|
---|
[15] | 200 | void VrpnClient_impl::Run(void) {
|
---|
[136] | 201 | struct timeval timeout;
|
---|
| 202 | timeout.tv_sec=0;
|
---|
| 203 | timeout.tv_usec=100000;
|
---|
[448] | 204 | uint16_t vrpnliteCounter=0;
|
---|
[136] | 205 |
|
---|
[15] | 206 | while (!self->ToBeStopped()) {
|
---|
[309] | 207 | if (connectionType==VrpnClient::Xbee) {
|
---|
[15] | 208 | ssize_t read = 0;
|
---|
| 209 | uint8_t response[38] = {0};
|
---|
[3] | 210 |
|
---|
[15] | 211 | read = serialport->Read(response, sizeof(response));
|
---|
| 212 | if (read > 0 && read != sizeof(response))
|
---|
| 213 | read += serialport->Read(&response[read], sizeof(response) - read);
|
---|
| 214 | // int temps=(float)self->GetTime()/(1000*1000);
|
---|
| 215 | // self->Printf("%i %i %i\n",temps-last,temps,last);
|
---|
| 216 | // last=temps;
|
---|
| 217 | if (read < 0) {
|
---|
| 218 | // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
|
---|
| 219 | } else if (read != sizeof(response)) {
|
---|
[294] | 220 | self->Warn("serial read error %i/%i\n", read, sizeof(response));
|
---|
[15] | 221 | } else {
|
---|
| 222 | // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
|
---|
| 223 | // printf("\n");
|
---|
| 224 | uint8_t checksum = 0;
|
---|
| 225 | for (ssize_t i = 3; i < read; i++)
|
---|
| 226 | checksum += response[i];
|
---|
| 227 | if (checksum != 255) {
|
---|
| 228 | self->Err("checksum error\n");
|
---|
| 229 | } else {
|
---|
| 230 | vrpn_TRACKERCB t;
|
---|
| 231 | float pos[3];
|
---|
| 232 | float quat[4];
|
---|
| 233 | uint8_t id = response[8];
|
---|
[3] | 234 |
|
---|
[15] | 235 | mutex->GetMutex();
|
---|
[332] | 236 | if (id < xbeeObjects.size()) {
|
---|
[15] | 237 | memcpy(pos, &response[9], sizeof(pos));
|
---|
| 238 | memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
|
---|
| 239 | for (int i = 0; i < 3; i++)
|
---|
| 240 | t.pos[i] = pos[i];
|
---|
| 241 | for (int i = 0; i < 4; i++)
|
---|
| 242 | t.quat[i] = quat[i];
|
---|
| 243 | if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
|
---|
[136] | 244 | Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
|
---|
[15] | 245 | } else {
|
---|
| 246 | // self->Printf("%i %f %f %f
|
---|
| 247 | // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
|
---|
[332] | 248 | VrpnObject_impl::handle_pos(xbeeObjects.at(id).vrpnobject, t);
|
---|
[3] | 249 | }
|
---|
[15] | 250 | }
|
---|
| 251 | mutex->ReleaseMutex();
|
---|
[3] | 252 | }
|
---|
[15] | 253 | }
|
---|
[309] | 254 | } else if(connectionType==VrpnClient::Vrpn) {
|
---|
[139] | 255 | if(connection->connected()==vrpn_true && !isConnected) {
|
---|
| 256 | isConnected=true;
|
---|
[294] | 257 | Printf("VRPN connected to %s\n",address.c_str());
|
---|
[139] | 258 | }
|
---|
| 259 | if(connection->connected()==vrpn_false && isConnected) {
|
---|
| 260 | isConnected=false;
|
---|
[294] | 261 | Printf("VRPN disconnected to %s\n",address.c_str());
|
---|
[139] | 262 | }
|
---|
| 263 | //timeout in mainloop is not well handled if not connected...
|
---|
| 264 | if(isConnected) {
|
---|
| 265 | //this is when trackables callbacks are called:
|
---|
| 266 | if(connection->mainloop(&timeout)!=0) {
|
---|
[294] | 267 | self->Warn("connection dropped from\n",address.c_str());
|
---|
[139] | 268 | }
|
---|
| 269 | //printf("%lld\n",GetTime()/(1000*1000));
|
---|
| 270 | mutex->GetMutex();
|
---|
[330] | 271 | for (unsigned int i = 0; i < trackables.size(); i++) {
|
---|
| 272 | // Printf("tracker %i\n",i);
|
---|
| 273 | trackables.at(i)->tracker->mainloop();
|
---|
| 274 | //Printf("tracker %i ok\n",i);
|
---|
| 275 | }
|
---|
[139] | 276 | mutex->ReleaseMutex();
|
---|
| 277 | } else {
|
---|
| 278 | connection->mainloop();
|
---|
| 279 | self->SleepMS(10);
|
---|
| 280 | }
|
---|
[309] | 281 | }else if(connectionType==VrpnClient::VrpnLite) {
|
---|
[330] | 282 | mutex->GetMutex();
|
---|
| 283 |
|
---|
| 284 | int16_t pos[3];
|
---|
| 285 | int16_t quat[4];
|
---|
[309] | 286 | Time time;
|
---|
[332] | 287 | char datas[trackables.size()*(sizeof(pos)+sizeof(quat))+ sizeof(time)];
|
---|
[330] | 288 | char *datasPtr=datas;
|
---|
| 289 |
|
---|
[309] | 290 | int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
|
---|
[330] | 291 | if(rcv!=sizeof(datas)) {
|
---|
| 292 | if(rcv>0) Printf("discarding message (size %i/%i)\n",rcv,sizeof(datas));
|
---|
| 293 | mutex->ReleaseMutex();
|
---|
| 294 | continue;
|
---|
| 295 | }
|
---|
[309] | 296 |
|
---|
[330] | 297 | memcpy(&time, datasPtr+sizeof(datas)-sizeof(time), sizeof(time));
|
---|
[309] | 298 | dataSocket->NetworkToHost((char*)(&time),sizeof(time));
|
---|
[330] | 299 |
|
---|
[332] | 300 | for (vector<VrpnObject_impl*>::iterator it = trackables.begin();it < trackables.end(); it++) {
|
---|
[330] | 301 | memcpy(pos, datasPtr, sizeof(pos));
|
---|
| 302 | datasPtr+=sizeof(pos);
|
---|
| 303 | memcpy(quat,datasPtr, sizeof(quat));
|
---|
| 304 | datasPtr+=sizeof(quat);
|
---|
| 305 |
|
---|
| 306 | for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
|
---|
| 307 | for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
|
---|
| 308 |
|
---|
| 309 | vrpn_TRACKERCB t;
|
---|
| 310 | for (int i = 0; i < 3; i++) t.pos[i] = ConvertPosition(pos[i]);
|
---|
[309] | 311 | // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
|
---|
| 312 | // flair::core::Quaternion
|
---|
[330] | 313 | t.quat[0] = ConvertQuaternion(quat[1]);
|
---|
| 314 | t.quat[1] = ConvertQuaternion(quat[2]);
|
---|
| 315 | t.quat[2] = ConvertQuaternion(quat[3]);
|
---|
| 316 | t.quat[3] = ConvertQuaternion(quat[0]);
|
---|
[309] | 317 | t.msg_time.tv_sec=time/((Time)1000000000);
|
---|
| 318 | t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
|
---|
| 319 | //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
|
---|
[332] | 320 | VrpnObject_impl::handle_pos((void*)(*it), t);
|
---|
[309] | 321 | }
|
---|
[330] | 322 |
|
---|
[309] | 323 | mutex->ReleaseMutex();
|
---|
[448] | 324 |
|
---|
| 325 | if(trackables.size()!=0) {
|
---|
| 326 | vrpnliteCounter++;//send ack to monitor connection on vrpnlite
|
---|
| 327 | //on bth, vrpn is configured to 50Hz, send ack at 1Hz
|
---|
| 328 | if(vrpnliteCounter==50) {
|
---|
| 329 | vrpnliteCounter=0;
|
---|
| 330 | char data=0;
|
---|
| 331 | dataSocket->SendMessage(&data,sizeof(char));
|
---|
| 332 | }
|
---|
| 333 | }
|
---|
[3] | 334 | }
|
---|
[15] | 335 | }
|
---|
[3] | 336 | }
|
---|
[330] | 337 |
|
---|
| 338 | float VrpnClient_impl::ConvertPosition(int16_t value) const {
|
---|
| 339 | return (float)value/1000.;
|
---|
| 340 | }
|
---|
| 341 |
|
---|
| 342 | float VrpnClient_impl::ConvertQuaternion(int16_t value) const {
|
---|
| 343 | return (float)value/32767.;
|
---|
| 344 | }
|
---|