ObsAvoiAlgoLIP.h
Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00029
00030
00031
00032
00033 #ifndef __OBS_AVOI_ALGO_LIP_H
00034 #define __OBS_AVOI_ALGO_LIP_H
00035
00036 #ifdef MIP_HOST_APPLE
00037 #include <applePatch.h>
00038 #endif
00039
00040 #include <algorithm>
00041 #include "ObsAvoiAlgo.h"
00042 #include <Spaces.h>
00043 #include <Scan.h>
00044 #include <DraWin.h>
00045
00046 using namespace MipResources;
00047
00048 namespace MipAlgorithms{
00049
00050
00052
00053
00054
00073
00074 class ObsAvoiAlgoLIPOptions : public Options {
00075 public:
00076 DecimalOption* driveGain;
00077 DecimalOption* turnrateGain;
00078 DecimalOption* influenceRadius;
00079 DecimalOption* driveSatDw;
00080 DecimalOption* turnrateSatDw;
00081 DecimalOption* driveSatUp;
00082 DecimalOption* turnSatUp;
00083 DecimalOption* obstacleGrowing;
00084 DecimalOption* potentialTypeChangeDistance;
00085 DecimalOption* turnOffDistance;
00086 DecimalOption* angClusterAmp ;
00087 DecimalOption* minimaObstacleDistance;
00088 DecimalOption* turnrateVortexField;
00089 BoolOption* vortexField;
00090 ObsAvoiAlgoLIPOptions();
00091
00092 string getObjectName() const {
00093 return "ObsAvoiAlgoLIPOptions";
00094 }
00095
00096 };
00097
00098
00102 class ObsAvoiAlgoLIPParams : public MIPObject{
00103 ObsAvoiAlgoLIPOptions _options;
00104
00105 public:
00106 DraWin* draWin;
00107 Decimal driveGain;
00108 Decimal turnrateGain;
00109 Decimal influenceRadius;
00110 Decimal driveSatDw;
00111 Decimal turnrateSatDw;
00112 Decimal driveSatUp;
00113 Decimal turnSatUp;
00114 Decimal obstacleGrowing;
00115 Decimal potentialTypeChangeDistance;
00116 Decimal turnOffDistance;
00117 Angle angClusterAmp;
00118 Decimal minimaObstacleDistance;
00119 bool vortexField;
00120 Decimal turnrateVortexField;
00121
00122
00139
00140 ObsAvoiAlgoLIPParams(DraWin* dW=0,Decimal dG=1.0,Decimal tG=1.0,Decimal iR=1.0,Decimal dSDw=0.1,Decimal trSDw=0.05,Decimal dSU=10.0,Decimal trSU=10.0,Decimal oG=0.1,Decimal pTCD=0.5,Decimal GoalDistanceOFF=0.01, Angle angClusterAmp =0.0,Decimal mOd=0.01,bool vortexField=false,Decimal tGV=2.0);
00141
00146 ObsAvoiAlgoLIPParams(DraWin* dW, int argc, const char* argv[]);
00147
00149 string getObjectName() const {
00150 return "ObsAvoiAlgoLIP";
00151 }
00152 };
00153
00157 class ObsAvoiAlgoLIPVars {
00158
00159 public:
00160 unsigned int drawList;
00161 vector<Position> localMinima;
00162 Pose robotPose;
00163 Position goal;
00164 Position totalForce;
00165 Position goalForce;
00166 Position repulsiveForce;
00167 Rays r;
00169 ObsAvoiAlgoLIPVars(){}
00170 };
00171
00172
00176 class ObsAvoiAlgoLIP : public ObsAvoiAlgo{
00177 private:
00178 ObsAvoiAlgoLIPVars _var;
00179 ObsAvoiAlgoLIPParams _par;
00180
00181 stringstream _ssdebug;
00182
00183
00184 Rays _Filtering(Scan &scan);
00185 void _draw();
00186 Position _goalForce(Position goal,Pose q);
00187 void _localMinimaFinder(Scan &scan,ObsAvoiAlgoIn in);
00188 Position _repulsiveForce(Decimal driveSat,Pose robotPose, vector<Position> &repulsivePoints,Pose sensPose);
00189 void _cmdSaturation(ObsAvoiAlgoOut &out);
00190 int minimaIndex(Ray data1,Ray data2,Ray data3,Ray data4);
00191 public:
00194 ObsAvoiAlgoLIP(ObsAvoiAlgoLIPParams inParams);
00195
00199 void step(ObsAvoiAlgoIn in, ObsAvoiAlgoOut &out);
00200
00201 void getControl(ObsAvoiAlgoIn &in, ObsAvoiAlgoOut *out){
00202 this->step(in, *out);
00203 }
00204
00206 string getObjectName() const {
00207 return "ObsAvoiAlgoLIP";
00208 }
00209 };
00210
00211
00212
00213
00214 };
00215
00216
00217 #endif
00218
00219
00220
00221
00222
00223
00224