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

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

sources reformatted with flair-format-dir script

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