source: flair-src/trunk/lib/FlairSensorActuator/src/VrpnObject_impl.cpp

Last change on this file was 330, checked in by Sanahuja Guillaume, 2 years ago

use less bandwidth in vprnlite

File size: 6.6 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:   VrpnObject.cpp
7//
8//  author:     César Richard, Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    objet vrpn
14//
15//
16/*********************************************************************/
17#include "VrpnObject_impl.h"
18#include "VrpnObject.h"
19#include "VrpnClient.h"
20#include "VrpnClient_impl.h"
21#include <vrpn_Tracker.h>
22#include <fcntl.h>
23#include <errno.h>
24#include <string.h>
25#include <unistd.h>
26#include <vrpn_Connection.h>
27#include <Matrix.h>
28#include <Tab.h>
29#include <TabWidget.h>
30#include <DataPlot1D.h>
31#include <OneAxisRotation.h>
32#include <Vector3D.h>
33#include <Euler.h>
34#include <math.h>
35
36using std::string;
37using namespace flair::core;
38using namespace flair::gui;
39using namespace flair::sensor;
40
41VrpnObject_impl::VrpnObject_impl(VrpnObject *self,
42                                 string name, int id, const TabWidget *tab,VrpnClient *client) {
43  parent = client;
44  this->self = self;
45
46        if(parent==NULL) {
47                self->Err("VrpnClient must be instanced before creating VrpnObject\n");
48                return;
49        }
50  if (id == -1 && GetVrpnClient()->ConnectionType()==VrpnClient::Xbee) {
51    self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
52  }
53  if (id != -1 && GetVrpnClient()->ConnectionType()==VrpnClient::VrpnLite) {
54    self->Err("identifiant pour la connexion ignore car inutile en mode VrpnLite\n");
55  }
56  if (id != -1 && GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn) {
57    self->Warn("identifiant pour la connexion ignore car inutile en mode Vrpn\n");
58  }
59 
60  // state
61  MatrixDescriptor *desc = new MatrixDescriptor(7, 1);
62  desc->SetElementName(0, 0, "q0");
63  desc->SetElementName(1, 0, "q1");
64  desc->SetElementName(2, 0, "q2");
65  desc->SetElementName(3, 0, "q3");
66  desc->SetElementName(4, 0, "x");
67  desc->SetElementName(5, 0, "y");
68  desc->SetElementName(6, 0, "z");
69  output = new Matrix(self, desc, floatType);
70  delete desc;
71
72  desc = new MatrixDescriptor(3, 1);
73  desc->SetElementName(0, 0, "roll");
74  desc->SetElementName(1, 0, "pitch");
75  desc->SetElementName(2, 0, "yaw");
76  state = new Matrix(self, desc, floatType);
77  delete desc;
78
79  // ui
80  plot_tab = new Tab(tab, "Mesures " + name);
81  x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
82  x_plot->AddCurve(output->Element(4));
83  y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
84  y_plot->AddCurve(output->Element(5));
85  z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
86  z_plot->AddCurve(output->Element(6));
87 
88  if (GetVrpnClient()->ConnectionType()==VrpnClient::Xbee) {
89    parent->pimpl_->AddTrackable(this, id);
90  } else if(GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn){
91    tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
92    tracker->register_change_handler(this, handle_pos);
93    tracker->shutup = true;
94    parent->pimpl_->AddTrackable(this);
95  } else if(GetVrpnClient()->ConnectionType()==VrpnClient::VrpnLite){
96    parent->pimpl_->AddTrackable(this);
97  }
98 
99  previousTime=TIME_INFINITE;
100}
101
102VrpnObject_impl::~VrpnObject_impl(void) {
103  parent->pimpl_->RemoveTrackable(this);
104  if (GetVrpnClient()->ConnectionType()==VrpnClient::Vrpn) {
105    tracker->unregister_change_handler(this, handle_pos);
106    delete tracker;
107  }
108  delete plot_tab;
109}
110
111bool VrpnObject_impl::IsTracked(unsigned int timeout_ms) {
112  output->GetMutex();
113  Time a = GetTime();
114  Time dt = a - output->DataTime();
115  output->ReleaseMutex();
116
117  if (dt > (Time)(timeout_ms * 1000000)) {
118    // self->Printf("%lld %lld %lld
119    // %lld\n",a,output->DataTime(),dt,(Time)(timeout_ms*1000000));
120    return false;
121  } else {
122    return true;
123  }
124}
125
126void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
127  output->GetMutex();
128  quaternion.q0 = output->ValueNoMutex(0, 0);
129  quaternion.q1 = output->ValueNoMutex(1, 0);
130  quaternion.q2 = output->ValueNoMutex(2, 0);
131  quaternion.q3 = output->ValueNoMutex(3, 0);
132  output->ReleaseMutex();
133}
134
135void VrpnObject_impl::GetPosition(Vector3Df &point) {
136  output->GetMutex();
137  point.x = output->ValueNoMutex(4, 0);
138  point.y = output->ValueNoMutex(5, 0);
139  point.z = output->ValueNoMutex(6, 0);
140  output->ReleaseMutex();
141}
142
143void VRPN_CALLBACK
144VrpnObject_impl::handle_pos(void *userdata, const vrpn_TRACKERCB t) {
145  bool is_nan = false;
146  VrpnObject_impl *caller = reinterpret_cast<VrpnObject_impl *>(userdata);
147  Time time = GetTime();
148  //Printf("%s %lld %lld\n",caller->self->ObjectName().c_str(),time,(Time)t.msg_time.tv_sec*(Time)1000000+(Time)t.msg_time.tv_usec);
149  Time vrpnTime=(Time)t.msg_time.tv_sec*(Time)1000000000+(Time)t.msg_time.tv_usec*(Time)1000;
150  Time deltaTime;
151  if(caller->previousTime!=TIME_INFINITE) {
152    deltaTime=vrpnTime-caller->previousTime;
153  } else {
154    deltaTime=TIME_INFINITE;
155  }
156  caller->previousTime=vrpnTime;
157 
158  // check if something is nan
159  for (int i = 0; i < 3; i++) {
160    if (isnan(t.pos[i]) == true)
161      is_nan = true;
162  }
163  for (int i = 0; i < 4; i++) {
164    if (isnan(t.quat[i]) == true)
165      is_nan = true;
166  }
167  if (is_nan == true) {
168    caller->self->Warn("data is nan, skipping it (time %lld)\n", time);
169    return;
170  }
171
172  // on prend une fois pour toute le mutex et on fait des accès directs
173  caller->output->GetMutex();
174
175  // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
176  // flair::core::Quaternion
177        Quaternion quaternion(t.quat[3],t.quat[0],t.quat[1],t.quat[2]);
178  Vector3Df pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
179
180  // on effectue les rotation
181  caller->parent->pimpl_->ComputeRotations(pos);
182  caller->parent->pimpl_->ComputeRotations(quaternion);
183
184  caller->output->SetValueNoMutex(0, 0, quaternion.q0);
185  caller->output->SetValueNoMutex(1, 0, quaternion.q1);
186  caller->output->SetValueNoMutex(2, 0, quaternion.q2);
187  caller->output->SetValueNoMutex(3, 0, quaternion.q3);
188  caller->output->SetValueNoMutex(4, 0, pos.x);
189  caller->output->SetValueNoMutex(5, 0, pos.y);
190  caller->output->SetValueNoMutex(6, 0, pos.z);
191
192  caller->output->SetDataTime(time,deltaTime);
193  //Printf("%s data filled\n",caller->self->ObjectName().c_str());
194  caller->output->ReleaseMutex();
195
196        Euler euler=quaternion.ToEuler();
197  caller->state->GetMutex();
198  caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));
199  caller->state->SetValueNoMutex(1, 0, Euler::ToDegree(euler.pitch));
200  caller->state->SetValueNoMutex(2, 0, Euler::ToDegree(euler.yaw));
201  caller->state->ReleaseMutex();
202//Printf("%s process\n",caller->self->ObjectName().c_str());
203  caller->self->ProcessUpdate(caller->output);
204  //Printf("%s process ok\n",caller->self->ObjectName().c_str());
205}
Note: See TracBrowser for help on using the repository browser.