[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,
|
---|
[136] | 42 | std::string address) {
|
---|
[15] | 43 | this->self = self;
|
---|
[294] | 44 | this->address = address;
|
---|
[309] | 45 | isConnected=false;
|
---|
| 46 | connectionType=VrpnClient::Vrpn;
|
---|
[3] | 47 |
|
---|
[15] | 48 | connection = vrpn_get_connection_by_name(address.c_str());
|
---|
[3] | 49 |
|
---|
[309] | 50 | CommonConstructor(name);
|
---|
[294] | 51 |
|
---|
| 52 | Printf("Connecting to VRPN server on %s\n",address.c_str());
|
---|
[3] | 53 | }
|
---|
| 54 |
|
---|
[309] | 55 | VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
|
---|
| 56 | uint16_t port) {
|
---|
| 57 | this->self = self;
|
---|
| 58 | isConnected=false;
|
---|
| 59 | connectionType=VrpnClient::VrpnLite;
|
---|
| 60 |
|
---|
| 61 | dataSocket =new UdpSocket(getFrameworkManager(), "data_socket", port);
|
---|
| 62 |
|
---|
| 63 | CommonConstructor(name);
|
---|
| 64 |
|
---|
| 65 | Printf("Connecting to VRPN-lite server on port %i\n",port);
|
---|
| 66 | }
|
---|
| 67 |
|
---|
| 68 | VrpnClient_impl::VrpnClient_impl(VrpnClient *self, string name,
|
---|
[15] | 69 | SerialPort *serialport, uint16_t us_period) {
|
---|
| 70 | this->us_period = us_period;
|
---|
| 71 | this->self = self;
|
---|
| 72 | this->serialport = serialport;
|
---|
[309] | 73 | connectionType=VrpnClient::Xbee;
|
---|
[3] | 74 |
|
---|
[15] | 75 | serialport->SetBaudrate(111111);
|
---|
| 76 | serialport->SetRxTimeout(us_period * 1000);
|
---|
[3] | 77 |
|
---|
[309] | 78 | CommonConstructor(name);
|
---|
| 79 |
|
---|
| 80 | Printf("Connecting to VRPN server through xbee on %s\n",serialport->ObjectName().c_str());
|
---|
| 81 | }
|
---|
| 82 |
|
---|
| 83 | void VrpnClient_impl::CommonConstructor(std::string name) {
|
---|
| 84 | mutex = new Mutex(self, name);
|
---|
| 85 |
|
---|
[15] | 86 | // station sol
|
---|
| 87 | main_tab = new Tab(getFrameworkManager()->GetTabWidget(), name);
|
---|
| 88 | tab = new TabWidget(main_tab->NewRow(), name);
|
---|
| 89 | setup_tab = new Tab(tab, "Reglages");
|
---|
[3] | 90 |
|
---|
[187] | 91 | rotation_1 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 1",OneAxisRotation::PreRotation);
|
---|
| 92 | rotation_2 = new OneAxisRotation(setup_tab->NewRow(), "post rotation 2",OneAxisRotation::PreRotation);
|
---|
[3] | 93 | }
|
---|
| 94 |
|
---|
[15] | 95 | VrpnClient_impl::~VrpnClient_impl() {
|
---|
[309] | 96 | if (connectionType==VrpnClient::Vrpn) {
|
---|
[140] | 97 | // on fait une copie car le delete touche a trackables_copy via
|
---|
[15] | 98 | // RemoveTrackable
|
---|
[140] | 99 | vector<VrpnObject_impl *> trackables_copy = trackables;
|
---|
[15] | 100 | for (unsigned int i = 0; i < trackables_copy.size(); i++)
|
---|
[140] | 101 | delete trackables_copy.at(i)->self;
|
---|
[15] | 102 | // trackables.clear();
|
---|
[309] | 103 | } else if(connectionType==VrpnClient::Xbee || connectionType==VrpnClient::VrpnLite) {
|
---|
[15] | 104 | // on fait une copie car le delete touche a xbee_objects_copy via
|
---|
| 105 | // RemoveTrackable
|
---|
[309] | 106 | vector<liteObject_t> liteObjects_copy = liteObjects;
|
---|
| 107 | for (unsigned int i = 0; i < liteObjects_copy.size(); i++)
|
---|
| 108 | delete liteObjects_copy.at(i).vrpnobject->self;
|
---|
[15] | 109 | }
|
---|
[3] | 110 |
|
---|
[15] | 111 | delete main_tab;
|
---|
[3] | 112 |
|
---|
[309] | 113 | if (connectionType==VrpnClient::Vrpn) {
|
---|
[15] | 114 | // it will automatically delete connection
|
---|
| 115 | connection->removeReference();
|
---|
| 116 | }
|
---|
[3] | 117 | }
|
---|
| 118 |
|
---|
[167] | 119 | void VrpnClient_impl::ComputeRotations(Vector3Df &point) {
|
---|
[15] | 120 | rotation_1->ComputeRotation(point);
|
---|
| 121 | rotation_2->ComputeRotation(point);
|
---|
[3] | 122 | }
|
---|
| 123 |
|
---|
[15] | 124 | void VrpnClient_impl::ComputeRotations(Quaternion &quat) {
|
---|
| 125 | rotation_1->ComputeRotation(quat);
|
---|
| 126 | rotation_2->ComputeRotation(quat);
|
---|
[3] | 127 | }
|
---|
| 128 |
|
---|
[140] | 129 | void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj) {
|
---|
[309] | 130 | if (connectionType==VrpnClient::Vrpn) {
|
---|
| 131 | mutex->GetMutex();
|
---|
| 132 | trackables.push_back(obj);
|
---|
| 133 | mutex->ReleaseMutex();
|
---|
| 134 | } else {
|
---|
| 135 | self->Warn("AddTrackable called but not in vrpn mode\n");
|
---|
| 136 | }
|
---|
[3] | 137 | }
|
---|
| 138 |
|
---|
[309] | 139 | void VrpnClient_impl::AddTrackable(VrpnObject_impl *obj, uint8_t id) {
|
---|
| 140 | if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
|
---|
| 141 | liteObject_t tmp;
|
---|
| 142 | tmp.vrpnobject = obj;
|
---|
| 143 | tmp.id = id;
|
---|
| 144 | mutex->GetMutex();
|
---|
| 145 | liteObjects.push_back(tmp);
|
---|
| 146 | mutex->ReleaseMutex();
|
---|
| 147 | } else {
|
---|
| 148 | self->Warn("AddTrackable called but not in vrpnlite nor in xbee mode\n");
|
---|
| 149 | }
|
---|
| 150 | }
|
---|
| 151 |
|
---|
[140] | 152 | void VrpnClient_impl::RemoveTrackable(VrpnObject_impl *obj) {
|
---|
[15] | 153 | mutex->GetMutex();
|
---|
[309] | 154 | if (connectionType==VrpnClient::Vrpn) {
|
---|
| 155 | for (vector<VrpnObject_impl *>::iterator it = trackables.begin();
|
---|
| 156 | it < trackables.end(); it++) {
|
---|
| 157 | if (*it == obj) {
|
---|
| 158 | trackables.erase(it);
|
---|
| 159 | break;
|
---|
| 160 | }
|
---|
[3] | 161 | }
|
---|
[15] | 162 | }
|
---|
[309] | 163 | if (connectionType==VrpnClient::VrpnLite || connectionType==VrpnClient::Xbee) {
|
---|
| 164 | for (vector<liteObject_t>::iterator it = liteObjects.begin();
|
---|
| 165 | it < liteObjects.end(); it++) {
|
---|
| 166 | if ((*it).vrpnobject == obj) {
|
---|
| 167 | liteObjects.erase(it);
|
---|
| 168 | break;
|
---|
| 169 | }
|
---|
[140] | 170 | }
|
---|
| 171 | }
|
---|
[15] | 172 | mutex->ReleaseMutex();
|
---|
[3] | 173 | }
|
---|
| 174 |
|
---|
[15] | 175 | void VrpnClient_impl::Run(void) {
|
---|
[136] | 176 | struct timeval timeout;
|
---|
| 177 | timeout.tv_sec=0;
|
---|
| 178 | timeout.tv_usec=100000;
|
---|
| 179 |
|
---|
[15] | 180 | while (!self->ToBeStopped()) {
|
---|
[309] | 181 | if (connectionType==VrpnClient::Xbee) {
|
---|
[15] | 182 | ssize_t read = 0;
|
---|
| 183 | uint8_t response[38] = {0};
|
---|
[3] | 184 |
|
---|
[15] | 185 | read = serialport->Read(response, sizeof(response));
|
---|
| 186 | if (read > 0 && read != sizeof(response))
|
---|
| 187 | read += serialport->Read(&response[read], sizeof(response) - read);
|
---|
| 188 | // int temps=(float)self->GetTime()/(1000*1000);
|
---|
| 189 | // self->Printf("%i %i %i\n",temps-last,temps,last);
|
---|
| 190 | // last=temps;
|
---|
| 191 | if (read < 0) {
|
---|
| 192 | // self->Warn("erreur rt_dev_read (%s)\n",strerror(-read));
|
---|
| 193 | } else if (read != sizeof(response)) {
|
---|
[294] | 194 | self->Warn("serial read error %i/%i\n", read, sizeof(response));
|
---|
[15] | 195 | } else {
|
---|
| 196 | // for(ssize_t i=0;i<read;i++) printf("%x ",response[i]);
|
---|
| 197 | // printf("\n");
|
---|
| 198 | uint8_t checksum = 0;
|
---|
| 199 | for (ssize_t i = 3; i < read; i++)
|
---|
| 200 | checksum += response[i];
|
---|
| 201 | if (checksum != 255) {
|
---|
| 202 | self->Err("checksum error\n");
|
---|
| 203 | } else {
|
---|
| 204 | vrpn_TRACKERCB t;
|
---|
| 205 | float pos[3];
|
---|
| 206 | float quat[4];
|
---|
| 207 | uint8_t id = response[8];
|
---|
[3] | 208 |
|
---|
[15] | 209 | mutex->GetMutex();
|
---|
[309] | 210 | if (id < liteObjects.size()) {
|
---|
[15] | 211 | memcpy(pos, &response[9], sizeof(pos));
|
---|
| 212 | memcpy(quat, &response[9] + sizeof(pos), sizeof(quat));
|
---|
| 213 | for (int i = 0; i < 3; i++)
|
---|
| 214 | t.pos[i] = pos[i];
|
---|
| 215 | for (int i = 0; i < 4; i++)
|
---|
| 216 | t.quat[i] = quat[i];
|
---|
| 217 | if (fabs(pos[0] > 10) || fabs(pos[1] > 10) || fabs(pos[2] > 10)) {
|
---|
[136] | 218 | Printf("prob pos %f %f %f\n", pos[0], pos[1], pos[2]);
|
---|
[15] | 219 | } else {
|
---|
| 220 | // self->Printf("%i %f %f %f
|
---|
| 221 | // %f\n",id,pos[0],pos[1],pos[2],(float)self->GetTime()/(1000*1000));
|
---|
[309] | 222 | VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
|
---|
[3] | 223 | }
|
---|
[15] | 224 | }
|
---|
| 225 | mutex->ReleaseMutex();
|
---|
[3] | 226 | }
|
---|
[15] | 227 | }
|
---|
[309] | 228 | } else if(connectionType==VrpnClient::Vrpn) {
|
---|
[139] | 229 | if(connection->connected()==vrpn_true && !isConnected) {
|
---|
| 230 | isConnected=true;
|
---|
[294] | 231 | Printf("VRPN connected to %s\n",address.c_str());
|
---|
[139] | 232 | }
|
---|
| 233 | if(connection->connected()==vrpn_false && isConnected) {
|
---|
| 234 | isConnected=false;
|
---|
[294] | 235 | Printf("VRPN disconnected to %s\n",address.c_str());
|
---|
[139] | 236 | }
|
---|
| 237 | //timeout in mainloop is not well handled if not connected...
|
---|
| 238 | if(isConnected) {
|
---|
| 239 | //this is when trackables callbacks are called:
|
---|
| 240 | if(connection->mainloop(&timeout)!=0) {
|
---|
[294] | 241 | self->Warn("connection dropped from\n",address.c_str());
|
---|
[139] | 242 | }
|
---|
| 243 | //printf("%lld\n",GetTime()/(1000*1000));
|
---|
| 244 | mutex->GetMutex();
|
---|
| 245 | for (unsigned int i = 0; i < trackables.size(); i++)
|
---|
[140] | 246 | trackables.at(i)->tracker->mainloop();
|
---|
[139] | 247 | mutex->ReleaseMutex();
|
---|
| 248 | } else {
|
---|
| 249 | connection->mainloop();
|
---|
| 250 | self->SleepMS(10);
|
---|
| 251 | }
|
---|
[309] | 252 | }else if(connectionType==VrpnClient::VrpnLite) {
|
---|
| 253 | vrpn_TRACKERCB t;
|
---|
| 254 | float pos[3];
|
---|
| 255 | float quat[4];
|
---|
| 256 | Time time;
|
---|
| 257 | uint8_t id;
|
---|
| 258 | char datas[sizeof(id) + sizeof(pos)+sizeof(quat)+ sizeof(time)];
|
---|
| 259 | int rcv=dataSocket->RecvMessage(datas,sizeof(datas),50*1000*1000);
|
---|
| 260 | if(rcv!=sizeof(datas)) continue;
|
---|
| 261 | id = datas[0];
|
---|
| 262 | memcpy(pos, datas+sizeof(id), sizeof(pos));
|
---|
| 263 | memcpy(quat, datas +sizeof(id)+ sizeof(pos), sizeof(quat));
|
---|
| 264 | memcpy(&time, datas+sizeof(id) + sizeof(pos)+sizeof(quat), sizeof(time));
|
---|
| 265 |
|
---|
| 266 | for(int i=0;i<3;i++) dataSocket->NetworkToHost((char*)(&pos[i]),sizeof(pos[i]));
|
---|
| 267 | for(int i=0;i<4;i++) dataSocket->NetworkToHost((char*)(&quat[i]),sizeof(quat[i]));
|
---|
| 268 | dataSocket->NetworkToHost((char*)(&time),sizeof(time));
|
---|
| 269 |
|
---|
| 270 | mutex->GetMutex();
|
---|
| 271 | if (id < liteObjects.size()) {
|
---|
| 272 | for (int i = 0; i < 3; i++) t.pos[i] = pos[i];
|
---|
| 273 | // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
|
---|
| 274 | // flair::core::Quaternion
|
---|
| 275 | t.quat[0] = quat[1];
|
---|
| 276 | t.quat[1] = quat[2];
|
---|
| 277 | t.quat[2] = quat[3];
|
---|
| 278 | t.quat[3] = quat[0];
|
---|
| 279 | t.msg_time.tv_sec=time/((Time)1000000000);
|
---|
| 280 | t.msg_time.tv_usec=(time%((Time)1000000000))/((Time)1000);
|
---|
| 281 | //Printf("%i %lld %lld %lld\n",id,time,t.msg_time.tv_sec,t.msg_time.tv_usec);
|
---|
| 282 | VrpnObject_impl::handle_pos(liteObjects.at(id).vrpnobject, t);
|
---|
| 283 | }
|
---|
| 284 | mutex->ReleaseMutex();
|
---|
[3] | 285 | }
|
---|
[15] | 286 | }
|
---|
[3] | 287 | }
|
---|