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

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

modifs

File size: 9.8 KB
RevLine 
[122]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>
[165]17#include <V4LCamera.h>
[122]18#include <CvtColor.h>
19#include <OpticalFlow.h>
[155]20#include <OpticalFlowCompensated.h>
[122]21#include <OpticalFlowSpeed.h>
22#include <LowPassFilter.h>
[142]23#include <EulerDerivative.h>
[122]24#include <cvmatrix.h>
25#include <GridLayout.h>
26#include <DataPlot1D.h>
27#include <Tab.h>
28#include <TabWidget.h>
29#include <GroupBox.h>
30#include <DoubleSpinBox.h>
[124]31#include <PushButton.h>
[122]32#include <FrameworkManager.h>
33#include <MetaDualShock3.h>
34#include <Vector2D.h>
35#include <AhrsData.h>
36#include <Ahrs.h>
37#include <Pid.h>
38
39#include <stdio.h>
40
41using namespace std;
42using namespace flair::core;
43using namespace flair::gui;
44using namespace flair::filter;
45using namespace flair::meta;
46using namespace flair::sensor;
47
48DemoOpticalFlow::DemoOpticalFlow(TargetController *controller): UavStateMachine(controller) {
49  Uav* uav=GetUav();
50        if (uav->GetVerticalCamera() == NULL) {
[131]51                                Err("no vertical camera found\n");
[122]52        exit(1);
53    }
[124]54       
55        startOpticalflow=new PushButton(GetButtonsLayout()->NewRow(),"start optical flow");
[122]56
[124]57        greyCameraImage=new CvtColor(uav->GetVerticalCamera(),"gray",CvtColor::Conversion_t::ToGray);
[122]58
[124]59        uav->GetVerticalCamera()->UseDefaultPlot(greyCameraImage->Output()); // Le defaultPlot de la caméra peut afficher n'importe quoi?
[122]60
[124]61        //optical flow stack
[147]62  //opticalFlow= matrice de déplacements en pixels entre 2 images consécutives
63  opticalFlow=new OpticalFlow(greyCameraImage,uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique");
[156]64  opticalFlowCompensated=new OpticalFlowCompensated(opticalFlow,uav->GetAhrs(),uav->GetVerticalCamera()->GetLayout()->NewRow(),"flux optique compense");
[163]65  opticalFlowSpeedRaw=new OpticalFlowSpeed(opticalFlowCompensated,uav->GetAhrs(),uav->GetVerticalCamera()->GetLayout()->NewRow(),"vitesse du Flux Optique");
[147]66  //opticalFlowSpeed=vitesse de déplacement en pixels par seconde (moyenne sur tous les points et division par le delta T)
[155]67  cvmatrix* twoByOneOFS=new cvmatrix((const Thread*)this,2,1,floatType);
68  cvmatrix* twoByOneOFAR=new cvmatrix((const Thread*)this,2,1,floatType);
69  cvmatrix* twoByOneOFA=new cvmatrix((const Thread*)this,2,1,floatType);
70  opticalFlowSpeed=new LowPassFilter(opticalFlowSpeedRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Speed lowPass",twoByOneOFS);
71  opticalFlowAccelerationRaw=new EulerDerivative(opticalFlowSpeed,uav->GetVerticalCamera()->GetLayout()->NewRow(),"derivative",twoByOneOFAR);
72  opticalFlowAcceleration=new LowPassFilter(opticalFlowAccelerationRaw,uav->GetVerticalCamera()->GetLayout()->NewRow(),"Acceleration lowPass",twoByOneOFA);
[147]73 
74  getFrameworkManager()->AddDeviceToLog(opticalFlowSpeedRaw);
[122]75
[155]76  Tab* opticalFlowTab=new Tab(getFrameworkManager()->GetTabWidget(),"flux optique");
77  DataPlot1D* xVelocityPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x speed (px/s)",-250,250);
78  DataPlot1D* yVelocityPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y speed (px/s)",-250,250);
79  DataPlot1D* xFirstPointPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x movement first point",-25,25);
80  DataPlot1D* yFirstPointPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y movement first point",-25,25);
81//  DataPlot1D* xAccelerationPlot=new DataPlot1D(opticalFlowTab->NewRow(),"x_acceleration",-250,250);
82//  DataPlot1D* yAccelerationPlot=new DataPlot1D(opticalFlowTab->LastRowLastCol(),"y_acceleration",-250,250);
[122]83
[155]84  xVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(0,0));
85  xVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(0,0),DataPlot::Blue);
86  yVelocityPlot->AddCurve(opticalFlowSpeedRaw->Output()->Element(1,0));
87  yVelocityPlot->AddCurve(opticalFlowSpeed->Matrix()->Element(1,0),DataPlot::Blue);
88  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(0,0));
89  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(1,0),DataPlot::Blue);
90  xFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(2,0),DataPlot::Green);
[156]91  yFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(0,1));
92  yFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(1,1),DataPlot::Blue);
93  yFirstPointPlot->AddCurve(opticalFlowCompensated->GetFirstPointDisplacement()->Element(2,1),DataPlot::Green);
[155]94//  xAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(0,0));
95//  xAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(0,0),DataPlot::Blue);
96//  yAccelerationPlot->AddCurve(opticalFlowAccelerationRaw->Matrix()->Element(1,0));
97//  yAccelerationPlot->AddCurve(opticalFlowAcceleration->Matrix()->Element(1,0),DataPlot::Blue);
[122]98
[155]99  u_x=new Pid(setupLawTab->At(1,0),"u_x");
100  u_x->UseDefaultPlot(graphLawTab->NewRow());
101  u_y=new Pid(setupLawTab->At(1,1),"u_y");
102  u_y->UseDefaultPlot(graphLawTab->LastRowLastCol());
[122]103
[155]104  opticalFlowGroupBox=new GroupBox(GetJoystick()->GetTab()->NewRow(),"consignes fo");
105  maxXSpeed=new DoubleSpinBox(opticalFlowGroupBox->NewRow(),"debattement x"," m/s",-5,5,0.1,1);
106  maxYSpeed=new DoubleSpinBox(opticalFlowGroupBox->LastRowLastCol(),"debattement y"," m/s",-5,5,0.1,1);
[154]107 
108  Tab* opticalFlowRealTab=new Tab(getFrameworkManager()->GetTabWidget(),"real speed");
109  opticalFlowRealSpeed=new cvmatrix((const Thread*)this,2,1,floatType);
110  opticalFlowReference=new cvmatrix((const Thread*)this,2,1,floatType);
111  opticalFlowRealAcceleration=new cvmatrix((const Thread*)this,2,1,floatType);
112  DataPlot1D* xRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x speed (m/s)",-2,2);
[155]113  DataPlot1D* yRealVelocityPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y speed (m/s)",-2,2);
[154]114  DataPlot1D* xRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->NewRow(),"x acceleration (m/s2)",-2,2);
[155]115  DataPlot1D* yRealAccelerationPlot=new DataPlot1D(opticalFlowRealTab->LastRowLastCol(),"y acceleration (m/s2)",-2,2);
[154]116  xRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(0));
[155]117  xRealVelocityPlot->AddCurve(opticalFlowReference->Element(0),DataPlot::Blue,"consigne");
118  yRealVelocityPlot->AddCurve(opticalFlowRealSpeed->Element(1));
[154]119  yRealVelocityPlot->AddCurve(opticalFlowReference->Element(1),DataPlot::Blue,"consigne");
120  xRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(0));
121  yRealAccelerationPlot->AddCurve(opticalFlowRealAcceleration->Element(1));
[122]122
[155]123  customReferenceOrientation= new AhrsData(this,"reference");
124  uav->GetAhrs()->AddPlot(customReferenceOrientation,DataPlot::Yellow);
125  AddDataToControlLawLog(customReferenceOrientation);
[165]126 
127  flagCameraLost=false;
[122]128}
129
130void DemoOpticalFlow::SignalEvent(Event_t event) {
131    switch(event) {
132    case Event_t::EnteringControlLoop:
133        opticalFlowReference->SetValue(0,0,GetJoystick()->GetAxisValue(1)*maxXSpeed->Value());//joy axis 0 maps to x displacement
134        opticalFlowReference->SetValue(1,0,GetJoystick()->GetAxisValue(0)*maxYSpeed->Value());//joy axis 1 maps to y displacement
[154]135        float focal=271.76;
136        float z,dz;
137        AltitudeValues(z, dz);
138        float scale=z/focal;
139        opticalFlowRealSpeed->SetValue(0,0,opticalFlowSpeed->Output(0,0)*scale);
140        opticalFlowRealSpeed->SetValue(1,0,opticalFlowSpeed->Output(1,0)*scale);
141        opticalFlowRealAcceleration->SetValue(0,0,opticalFlowAcceleration->Output(0,0)*scale);
142        opticalFlowRealAcceleration->SetValue(1,0,opticalFlowAcceleration->Output(1,0)*scale);
[122]143        break;
144    }
145}
146
[124]147void DemoOpticalFlow::StartOpticalFlow(void) {
148        if (SetOrientationMode(OrientationMode_t::Custom)) {
149                        Thread::Info("(Re)entering optical flow mode\n");
150                        u_x->Reset();
151                        u_y->Reset();
152        } else {
153                        Thread::Warn("Could not enter optical flow mode\n");
154        }
155}
156
157void DemoOpticalFlow::ExtraCheckPushButton(void) {
158        if(startOpticalflow->Clicked()) {
[142]159            StartOpticalFlow();
[124]160        }
161}
162
[122]163void DemoOpticalFlow::ExtraCheckJoystick(void) {
164    static bool wasOpticalFlowModeButtonPressed=false;
165    // controller button R1 enters optical flow mode
166    if(GetJoystick()->IsButtonPressed(9)) { // R1
167        if (!wasOpticalFlowModeButtonPressed) {
168            wasOpticalFlowModeButtonPressed=true;
[124]169            StartOpticalFlow();
[122]170        }
171    } else {
172        wasOpticalFlowModeButtonPressed=false;
173    }
174}
175
176const AhrsData *DemoOpticalFlow::GetReferenceOrientation(void) {
177    Euler refAngles=GetDefaultReferenceOrientation()->GetQuaternion().ToEuler();//to keep default yaw reference
178
[143]179    // /!\ in this demo, the target value is a speed (in m/s). As a consequence the error is the difference between the current speed and the target speed
[122]180    Vector2D error, errorVariation; // in Uav coordinate system
[154]181
182    error.x=opticalFlowRealSpeed->Value(0,0)-opticalFlowReference->Value(0,0);
183    error.y=opticalFlowRealSpeed->Value(1,0)-opticalFlowReference->Value(1,0);
184    errorVariation.x=opticalFlowRealAcceleration->Value(0,0);
185    errorVariation.y=opticalFlowRealAcceleration->Value(1,0);
[143]186//Printf("Altitude=%f, Error=(%f,%f)\n",z,error.x,error.y);
[122]187
188    u_x->SetValues(error.x, errorVariation.x);
189    u_x->Update(GetTime());
190    refAngles.pitch=u_x->Output();
191
192    u_y->SetValues(error.y, errorVariation.y);
193    u_y->Update(GetTime());
194    refAngles.roll=-u_y->Output();
195
196    customReferenceOrientation->SetQuaternionAndAngularRates(refAngles.ToQuaternion(),Vector3D(0,0,0));
197
198    return customReferenceOrientation;
199}
200
[165]201void DemoOpticalFlow::ExtraSecurityCheck(void) {
202  if(GetUav()->GetType()=="hds_x8") {
203    if(((V4LCamera*)GetUav()->GetVerticalCamera())->HasProblems() && !flagCameraLost) {
204      flagCameraLost = true;
205      Thread::Err("Camera lost\n");
206      SafeStop();
207      Land();
208    }
209  }
210}
211
[122]212DemoOpticalFlow::~DemoOpticalFlow() {
213}
Note: See TracBrowser for help on using the repository browser.