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

Last change on this file since 278 was 275, checked in by Sanahuja Guillaume, 6 years ago

correct bug for vpn delta time

File size: 6.0 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) {
43 parent = GetVrpnClient();
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 && parent->UseXbee()) {
51 self->Err("erreur aucun identifiant specifie pour la connexion Xbee\n");
52 }
53 if (id != -1 && !parent->UseXbee()) {
54 self->Warn(
55 "identifiant pour la connexion Xbee ignore car pas en mode Xbee\n");
56 }
57
58 // state
59 cvmatrix_descriptor *desc = new cvmatrix_descriptor(7, 1);
60 desc->SetElementName(0, 0, "q0");
61 desc->SetElementName(1, 0, "q1");
62 desc->SetElementName(2, 0, "q2");
63 desc->SetElementName(3, 0, "q3");
64 desc->SetElementName(4, 0, "x");
65 desc->SetElementName(5, 0, "y");
66 desc->SetElementName(6, 0, "z");
67 output = new Matrix(self, desc, floatType);
68 delete desc;
69
70 desc = new cvmatrix_descriptor(3, 1);
71 desc->SetElementName(0, 0, "roll");
72 desc->SetElementName(1, 0, "pitch");
73 desc->SetElementName(2, 0, "yaw");
74 state = new Matrix(self, desc, floatType);
75 delete desc;
76
77 // ui
78 plot_tab = new Tab(tab, "Mesures " + name);
79 x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
80 x_plot->AddCurve(output->Element(4));
81 y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
82 y_plot->AddCurve(output->Element(5));
83 z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
84 z_plot->AddCurve(output->Element(6));
85
86 if (parent->UseXbee()) {
87 tracker = NULL;
88 parent->pimpl_->AddTrackable(this, id);
89 } else {
90 tracker = new vrpn_Tracker_Remote(name.c_str(), parent->pimpl_->connection);
91 tracker->register_change_handler(this, handle_pos);
92 tracker->shutup = true;
93 parent->pimpl_->AddTrackable(this);
94 }
95
96 previousTime=TIME_INFINITE;
97}
98
99VrpnObject_impl::~VrpnObject_impl(void) {
100 parent->pimpl_->RemoveTrackable(this);
101 if (tracker != NULL) {// normal
102 tracker->unregister_change_handler(this, handle_pos);
103 delete tracker;
104 }
105 delete plot_tab;
106}
107
108bool VrpnObject_impl::IsTracked(unsigned int timeout_ms) {
109 output->GetMutex();
110 Time a = GetTime();
111 Time dt = a - output->DataTime();
112 output->ReleaseMutex();
113
114 if (dt > (Time)(timeout_ms * 1000000)) {
115 // self->Printf("%lld %lld %lld
116 // %lld\n",a,output->DataTime(),dt,(Time)(timeout_ms*1000000));
117 return false;
118 } else {
119 return true;
120 }
121}
122
123void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
124 output->GetMutex();
125 quaternion.q0 = output->ValueNoMutex(0, 0);
126 quaternion.q1 = output->ValueNoMutex(1, 0);
127 quaternion.q2 = output->ValueNoMutex(2, 0);
128 quaternion.q3 = output->ValueNoMutex(3, 0);
129 output->ReleaseMutex();
130}
131
132void VrpnObject_impl::GetPosition(Vector3Df &point) {
133 output->GetMutex();
134 point.x = output->ValueNoMutex(4, 0);
135 point.y = output->ValueNoMutex(5, 0);
136 point.z = output->ValueNoMutex(6, 0);
137 output->ReleaseMutex();
138}
139
140void VRPN_CALLBACK
141VrpnObject_impl::handle_pos(void *userdata, const vrpn_TRACKERCB t) {
142 bool is_nan = false;
143 VrpnObject_impl *caller = reinterpret_cast<VrpnObject_impl *>(userdata);
144 Time time = GetTime();
145 //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);
146 Time vrpnTime=(Time)t.msg_time.tv_sec*(Time)1000000000+(Time)t.msg_time.tv_usec*(Time)1000;
147 Time deltaTime;
148 if(caller->previousTime!=TIME_INFINITE) {
149 deltaTime=vrpnTime-caller->previousTime;
150 } else {
151 deltaTime=TIME_INFINITE;
152 }
153 caller->previousTime=vrpnTime;
154
155 // check if something is nan
156 for (int i = 0; i < 3; i++) {
157 if (isnan(t.pos[i]) == true)
158 is_nan = true;
159 }
160 for (int i = 0; i < 4; i++) {
161 if (isnan(t.quat[i]) == true)
162 is_nan = true;
163 }
164 if (is_nan == true) {
165 caller->self->Warn("data is nan, skipping it (time %lld)\n", time);
166 return;
167 }
168
169 // on prend une fois pour toute le mutex et on fait des accès directs
170 caller->output->GetMutex();
171
172 // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
173 // flair::core::Quaternion
174 Quaternion quaternion(t.quat[3],t.quat[0],t.quat[1],t.quat[2]);
175 Vector3Df pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
176
177 // on effectue les rotation
178 caller->parent->pimpl_->ComputeRotations(pos);
179 caller->parent->pimpl_->ComputeRotations(quaternion);
180
181 caller->output->SetValueNoMutex(0, 0, quaternion.q0);
182 caller->output->SetValueNoMutex(1, 0, quaternion.q1);
183 caller->output->SetValueNoMutex(2, 0, quaternion.q2);
184 caller->output->SetValueNoMutex(3, 0, quaternion.q3);
185 caller->output->SetValueNoMutex(4, 0, pos.x);
186 caller->output->SetValueNoMutex(5, 0, pos.y);
187 caller->output->SetValueNoMutex(6, 0, pos.z);
188
189 caller->output->SetDataTime(time,deltaTime);
190 caller->output->ReleaseMutex();
191
192 Euler euler=quaternion.ToEuler();
193 caller->state->GetMutex();
194 caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));
195 caller->state->SetValueNoMutex(1, 0, Euler::ToDegree(euler.pitch));
196 caller->state->SetValueNoMutex(2, 0, Euler::ToDegree(euler.yaw));
197 caller->state->ReleaseMutex();
198
199 caller->self->ProcessUpdate(caller->output);
200}
Note: See TracBrowser for help on using the repository browser.