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

Last change on this file since 148 was 148, checked in by Sanahuja Guillaume, 5 years ago

m

File size: 5.6 KB
RevLine 
[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:   VrpnObject.cpp
7//
[122]8//  author:     César Richard, Guillaume Sanahuja
[3]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"
[15]21#include <vrpn_Tracker.h>
[3]22#include <fcntl.h>
23#include <errno.h>
24#include <string.h>
25#include <unistd.h>
26#include <vrpn_Connection.h>
27#include <cvmatrix.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
[122]41VrpnObject_impl::VrpnObject_impl(VrpnObject *self,
[15]42                                 string name, int id, const TabWidget *tab) {
[122]43  parent = GetVrpnClient();
[15]44  this->self = self;
[3]45
[122]46        if(parent==NULL) {
47                self->Err("VrpnClient must be instanced before creating VrpnObject\n");
48                return;
49        }
[15]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  }
[140]57 
[15]58  // state
[135]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");
[15]67  output = new cvmatrix(self, desc, floatType);
[148]68  delete desc;
[15]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 cvmatrix(self, desc, floatType);
[148]75  delete desc;
[15]76
77  // ui
78  plot_tab = new Tab(tab, "Mesures " + name);
79  x_plot = new DataPlot1D(plot_tab->NewRow(), "x", -10, 10);
[135]80  x_plot->AddCurve(output->Element(4));
[15]81  y_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "y", -10, 10);
[135]82  y_plot->AddCurve(output->Element(5));
[15]83  z_plot = new DataPlot1D(plot_tab->LastRowLastCol(), "z", -2, 0);
[135]84  z_plot->AddCurve(output->Element(6));
[139]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;
[140]93    parent->pimpl_->AddTrackable(this);
[139]94  }
[140]95 
[3]96}
[15]97
98VrpnObject_impl::~VrpnObject_impl(void) {
[140]99  parent->pimpl_->RemoveTrackable(this);
100  if (tracker != NULL) {// normal
[15]101    tracker->unregister_change_handler(this, handle_pos);
102    delete tracker;
103  }
104  delete plot_tab;
[3]105}
106
[15]107bool VrpnObject_impl::IsTracked(unsigned int timeout_ms) {
108  output->GetMutex();
109  Time a = GetTime();
110  Time dt = a - output->DataTime();
111  output->ReleaseMutex();
112
113  if (dt > (Time)(timeout_ms * 1000000)) {
114    // self->Printf("%lld %lld %lld
115    // %lld\n",a,output->DataTime(),dt,(Time)(timeout_ms*1000000));
116    return false;
117  } else {
118    return true;
119  }
[3]120}
121
122void VrpnObject_impl::GetQuaternion(Quaternion &quaternion) {
[15]123  output->GetMutex();
[135]124  quaternion.q0 = output->ValueNoMutex(0, 0);
125  quaternion.q1 = output->ValueNoMutex(1, 0);
126  quaternion.q2 = output->ValueNoMutex(2, 0);
127  quaternion.q3 = output->ValueNoMutex(3, 0);
[15]128  output->ReleaseMutex();
129}
[3]130
[15]131void VrpnObject_impl::GetPosition(Vector3D &point) {
132  output->GetMutex();
[135]133  point.x = output->ValueNoMutex(4, 0);
134  point.y = output->ValueNoMutex(5, 0);
135  point.z = output->ValueNoMutex(6, 0);
[15]136  output->ReleaseMutex();
137}
[3]138
[15]139void VRPN_CALLBACK
140VrpnObject_impl::handle_pos(void *userdata, const vrpn_TRACKERCB t) {
141  bool is_nan = false;
142  VrpnObject_impl *caller = reinterpret_cast<VrpnObject_impl *>(userdata);
143  Time time = GetTime();
[3]144
[15]145  // check if something is nan
146  for (int i = 0; i < 3; i++) {
147    if (isnan(t.pos[i]) == true)
148      is_nan = true;
149  }
150  for (int i = 0; i < 4; i++) {
151    if (isnan(t.quat[i]) == true)
152      is_nan = true;
153  }
154  if (is_nan == true) {
155    caller->self->Warn("data is nan, skipping it (time %lld)\n", time);
156    return;
157  }
[3]158
[122]159  // on prend une fois pour toute le mutex et on fait des accès directs
[15]160  caller->output->GetMutex();
[3]161
[15]162  // warning: t.quat is defined as (qx,qy,qz,qw), which is different from
163  // flair::core::Quaternion
[135]164        Quaternion quaternion(t.quat[3],t.quat[0],t.quat[1],t.quat[2]);
[15]165  Vector3D pos((float)t.pos[0], (float)t.pos[1], (float)t.pos[2]);
[3]166
[15]167  // on effectue les rotation
168  caller->parent->pimpl_->ComputeRotations(pos);
[135]169  caller->parent->pimpl_->ComputeRotations(quaternion);
[3]170
[135]171  caller->output->SetValueNoMutex(0, 0, quaternion.q0);
172  caller->output->SetValueNoMutex(1, 0, quaternion.q1);
173  caller->output->SetValueNoMutex(2, 0, quaternion.q2);
174  caller->output->SetValueNoMutex(3, 0, quaternion.q3);
175  caller->output->SetValueNoMutex(4, 0, pos.x);
176  caller->output->SetValueNoMutex(5, 0, pos.y);
177  caller->output->SetValueNoMutex(6, 0, pos.z);
[3]178
[15]179  caller->output->SetDataTime(time);
180  caller->output->ReleaseMutex();
[3]181
[135]182        Euler euler=quaternion.ToEuler();
[15]183  caller->state->GetMutex();
184  caller->state->SetValueNoMutex(0, 0, Euler::ToDegree(euler.roll));
185  caller->state->SetValueNoMutex(1, 0, Euler::ToDegree(euler.pitch));
186  caller->state->SetValueNoMutex(2, 0, Euler::ToDegree(euler.yaw));
187  caller->state->ReleaseMutex();
188
189  caller->self->ProcessUpdate(caller->output);
[3]190}
Note: See TracBrowser for help on using the repository browser.