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

Last change on this file since 102 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

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