source: flair-src/trunk/demos/OpticalFlow/uav/src/DemoOpticalFlow.cpp@ 123

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

modifs uav vrpn i686

File size: 6.4 KB
Line 
1// created: 2011/05/01
2// filename: DemoOpticalFlow.cpp
3//
4// author: Guillaume Sanahuja
5// Copyright Heudiasyc UMR UTC/CNRS 7253
6//
7// version: $Id: $
8//
9// purpose: demo optical flow
10//
11//
12/*********************************************************************/
13
14#include "DemoOpticalFlow.h"
15#include <Uav.h>
16#include <Camera.h>
17#include <CvtColor.h>
18#include <OpticalFlow.h>
19#include <OpticalFlowSpeed.h>
20#include <LowPassFilter.h>
21#include <cvmatrix.h>
22#include <GridLayout.h>
23#include <DataPlot1D.h>
24#include <Tab.h>
25#include <TabWidget.h>
26#include <GroupBox.h>
27#include <DoubleSpinBox.h>
28#include <FrameworkManager.h>
29#include <MetaDualShock3.h>
30#include <Vector2D.h>
31#include <AhrsData.h>
32#include <Ahrs.h>
33#include <Pid.h>
34
35#include <stdio.h>
36
37using namespace std;
38using namespace flair::core;
39using namespace flair::gui;
40using namespace flair::filter;
41using namespace flair::meta;
42using namespace flair::sensor;
43
44DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
45 Uav* uav=GetUav();
46 if (uav->GetVerticalCamera() == NULL) {
47 exit(1);
48 }
49uav->GetVerticalCamera()->SetLogFormat(Camera::LogFormat::JPG);
50getFrameworkManager()->AddDeviceToLog(uav->GetVerticalCamera());
51 greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::GRAY);
52
53 uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output()); // Le defaultPlot de la caméra peut afficher n'importe quoi?
54
55 //optical flow stack
56 opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
57 opticalFlowSpeed=new OpticalFlowSpeed(opticalFlow,"vitesse du flux optique");
58
59 cvmatrix_descriptor* desc=new cvmatrix_descriptor(2,1);
60 for(int i=0;i<2;i++) {
61 desc->SetElementName(i,0,opticalFlowSpeed->Output()->Name(i,0));
62 }
63 cvmatrix* prev_value=new cvmatrix((const Thread*)this,desc,floatType,uav->ObjectName()); // diamond inheritance
64 for(int i=0;i<2;i++) {
65 prev_value->SetValue(i,0,0);
66 }
67
68 opticalFlowSpeedFiltered=new LowPassFilter(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"passe bas",prev_value);
69// delete prev_value?
70
71 getFrameworkManager()->AddDeviceToLog(opticalFlowSpeed);
72
73 Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
74 DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_velocity",-5,5);
75 DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_velocity",-5,5);
76
77 xVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(0,0));
78 xVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(0,0),DataPlot::Blue);
79 yVelocityPlot->AddCurve(opticalFlowSpeed->Output()->Element(1,0));
80 yVelocityPlot->AddCurve(opticalFlowSpeedFiltered->Matrix()->Element(1,0),DataPlot::Blue);
81
82 u_x=new Pid(setupLawTab->At(1,0),"u_x");
83 u_x->UseDefaultPlot(graphLawTab->NewRow());
84 u_y=new Pid(setupLawTab->At(1,1),"u_y");
85 u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
86
87 opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
88 maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
89 maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
90
91 opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
92 xVelocityPlot->AddCurve(opticalFlowReference->Element(0,0),DataPlot::Green,"consigne");
93 yVelocityPlot->AddCurve(opticalFlowReference->Element(1,0),DataPlot::Green,"consigne");
94
95 customReferenceOrientation= new AhrsData(this,"reference");
96 uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
97 AddDataToControlLawLog(customReferenceOrientation);
98}
99
100void DemoOpticalFlow::SignalEvent(Event_t event) {
101 switch(event) {
102 case Event_t::EnteringControlLoop:
103 opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
104 opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
105 break;
106 }
107}
108
109void DemoOpticalFlow::ExtraCheckJoystick(void) {
110 static bool wasOpticalFlowModeButtonPressed=false;
111 // controller button R1 enters optical flow mode
112 if(GetJoystick()->IsButtonPressed(9)) { // R1
113 if (!wasOpticalFlowModeButtonPressed) {
114 wasOpticalFlowModeButtonPressed=true;
115 if (SetOrientationMode(OrientationMode_t::Custom)) {
116 Thread::Info("(Re)entering optical flow mode\n");
117 u_x->Reset();
118 u_y->Reset();
119 } else {
120 Thread::Warn("Could not enter optical flow mode\n");
121 }
122 }
123 } else {
124 wasOpticalFlowModeButtonPressed=false;
125 }
126}
127
128const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
129 Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
130
131 // /!\ in this demo, the target value is a speed (in pixel/s). As a consequence the error is the difference between the current speed and the target speed
132 Vector2D error, errorVariation; // in Uav coordinate system
133
134 //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
135 //opticalFlowSpeed=vitesse de déplacement en pixel par seconde (moyenne sur tous les points et division par le delta T)
136 error.x=opticalFlowSpeedFiltered->Output(0,0)-opticalFlowReference->Value(0,0);
137 error.y=opticalFlowSpeedFiltered->Output(1,0)-opticalFlowReference->Value(1,0);
138
139 //la dérivée est à la fréquence de la loi de commande ("rapide") alors que le flux optique est à la fréquence de la caméra
140 // fréquemment la dérivée car le signal n'a pas bougé -> dérivée super crade
141 //gsanahuj: brancher un eulerderivative derriere le opticalFlowSpeedFiltered pour avoir la derivee
142 //opticalFlowSpeed doit etre renomme car finalement ce n'est pas une vitesse mais un deplacement
143 errorVariation.x=0;
144 errorVariation.y=0;
145
146 u_x->SetValues(error.x, errorVariation.x);
147 u_x->Update(GetTime());
148 refAngles.pitch=u_x->Output();
149
150 u_y->SetValues(error.y, errorVariation.y);
151 u_y->Update(GetTime());
152 refAngles.roll=-u_y->Output();
153
154 customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
155
156 return customReferenceOrientation;
157}
158
159DemoOpticalFlow::~DemoOpticalFlow() {
160}
Note: See TracBrowser for help on using the repository browser.