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

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

sensoractuator

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