ParticleMutLocTask.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
00030
00035
00036
00037
00038 #ifndef __PARTICLE_MUT_LOC_TASK_H_
00039 #define __PARTICLE_MUT_LOC_TASK_H_
00040
00041
00042 #include <Task.h>
00043 #include <Resource.h>
00044 #include <MobileRob.h>
00045 #include <DraWin.h>
00046 #include <SharedMem.h>
00047 #include <Time.h>
00048 #include <SimTime.h>
00049
00050 #include <MultiRegCam.h>
00051 #include <CommonOptions.h>
00052 #include <MultiReg.h>
00053 #include <ProbMultiReg.h>
00054 #include <ProbMultiRegCam.h>
00055 #include <PaFilBFL.h>
00056 #include <PaFilBFLAbsRel.h>
00057 #include <PaFilBFLRelRel.h>
00058 #include <PaFilBFLRelRelBearOnly.h>
00059 #include <CommModule.h>
00060
00061
00062 using namespace MipAlgorithms;
00063 using namespace MipResources;
00064
00065
00066 namespace MipTasks{
00068
00069
00070
00077 enum FrameTypes{
00078 FIXED_FRAME,
00079 ATTACHED_FRAME,
00080 FRAME_TYPES_NUM
00081 };
00082
00083 static const char* FrameTypesNames[FRAME_TYPES_NUM] = {
00084 "fixed",
00085 "attached"
00086 };
00087
00088
00092 class ParticleMutLocOptions : public Options {
00093 public:
00094 DecimalOption* bLinearToll;
00095 DecimalOption* bAngularToll;
00096 IntOption* bObs0Trials;
00097 IntOption* bObs1Trials;
00098 IntOption* bMinInliersNum;
00099 DecimalOption* bPrewarpingToll;
00100
00101 DecimalOption* mLinearToll;
00102 DecimalOption* mAngularToll;
00103 DecimalOption* mMaxVisuale;
00104 DecimalOption* mDiscretizationStep;
00105
00106 DecimalOption* bmDiscretization;
00107
00108 IntOption* minObservationSize;
00109
00110 IntOption* findTarget;
00111 IntOption* targetId;
00112
00113 BoolOption* banIdActive;
00114
00115 StringOption* myFrameType;
00116 StringOption* hisFrameType;
00117 BoolOption* feedbackInMultiReg;
00118 BoolOption* bearingOnlyMeas;
00119
00120 DecimalOption* mcLinearToll;
00121 DecimalOption* mcAngularToll;
00122 DecimalOption* mcProjToll;
00123 BoolOption* mcDrawRealPos;
00124 IntOption* mcExpMethod;
00125
00126 BoolOption* saveEstimates;
00127 StringOption* saveFilesRoot;
00128 BoolOption* saveRealPos;
00129 BoolOption* saveMultiRegCam;
00130
00131 ParticleMutLocOptions();
00132
00133 OptionGroupsType getGroup();
00134
00135 string getObjectName() const {
00136 return "ParticleMutLoc";
00137 }
00138 };
00139
00140
00144 class ParticleMutLocSharedMem : public SharedMem{
00145 public:
00147 void initSharedMem();
00148
00150 Time getTime();
00151
00155 PosiFeatures getMyProtrusions(MotionModuleTState &state);
00156
00160 PosiFeatures getMyObservation(MotionModuleTState &state);
00161
00165 vector<Angle> getMyCamObs(MotionModuleTState &state);
00166
00169 void setFormation(PoseFeatures &form);
00170
00173 void setEstimates(PoseFeatures &est);
00174
00177 Pose getTarget();
00178
00181 void setTarget(Pose &t);
00182
00185 bool doMloc();
00186
00189 bool printFormation();
00190
00193 bool printFormationDetails();
00194
00197 vector<int> getAvoidParticleMutLocIds();
00198 };
00199
00200
00204 class ParticleMutLocTask : public Task{
00205 private:
00206 static const TaskPlate _plate = PML_TASK;
00207 static const long int _maxDurSec = 0, _maxDurUsec = 500000;
00208 static const long int _maxSamplPeriodSec = 1, _maxSamplPeriodUsec = 0;
00209 static const long int _minSamplPeriodSec = 0, _minSamplPeriodUsec = 400000;
00210
00211
00212 DraWin* _draWin;
00213 unsigned int _drawList;
00214 bool _dwOn;
00215 ParticleMutLocSharedMem* _sharedMem;
00216 CommModule* _commModule;
00217
00218 FrameTypes _myFrameType;
00219 FrameTypes _hisFrameType;
00220
00221
00222 ParticleMutLocOptions _options;
00223
00224
00225 MultiReg* _multiReg;
00226 ProbMultiReg* _probMultiReg;
00227 MultiRegCam* _multiRegCam;
00228 ProbMultiRegCam* _probMultiRegCam;
00229 map<int,MutLocFilter*> _filter;
00230 vector<int> _activatedFilters;
00231
00232 map<int,fstream*> _particleFiles;
00233
00234
00235 bool _domultireg;
00236 bool _dofilter;
00237 vector<int> _ignoreIds;
00238 bool _printFormation;
00239 bool _printFormationDetails;
00240
00241
00242 Time _myTime;
00243 int _myId;
00244 Pose _myPose;
00245 PosiFeatures _myPosiFeatures;
00246 Observation _myObservation;
00247 vector<Angle> _myCamObs;
00248
00249
00250 map<int,PosiFeatures> _othersPosiFeatures;
00251 vector<int> _othersPosiFeaturesNums;
00252 map<int, vector<Angle> > _othersCamObs;
00253 vector<int> _othersCamObsNums;
00254 map<int,Pose> _othersPose;
00255 vector<int> _othersPoseNums;
00256
00257
00258 map<int,Pose> _estimates;
00259 vector<int> _activatedEstimates;
00260 map<int,SimTimer> _timeFromLastMessage;
00261 PoseFeatures _formation;
00262
00263 fstream _savefile;
00264 fstream _saveRealPosFile;
00265 fstream _saveMultiRegCamFile;
00266 stringstream s;
00267
00268 public:
00270 ParticleMutLocTask(ResourcePointers resources,int argc, const char* argv[]);
00271
00273 ~ParticleMutLocTask();
00274
00277 TaskPlate getPlate() const {
00278 return _plate;
00279 }
00282 Time getMaxDuration(){
00283 return Time(_maxDurSec,_maxDurUsec);
00284 }
00287 Time getMaxSamplPeriod(){
00288 return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
00289 }
00292 Time getMinSamplPeriod(){
00293 return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
00294 }
00297 TaskOutputs run();
00298
00299 private:
00300
00302 void _readSharedMemory();
00303
00305 void _comunicate();
00306
00308 void _draw();
00309
00311 void _writeSharedMem();
00312
00313 void _computeFormation();
00314
00317 void _insertValidObservations(vector<Observation> &allObservations);
00318
00319 void _insertValidCamObservations(vector<AngleId> &allObservations);
00320
00321 void _insertValidCamOffObservations(vector<AngleId> &allObservations);
00322
00324 void eraseDupCamInput(vector<AngleId> &input);
00325
00329 void _completeMutLocFilInput(MutLocFilInput &fStepIn, int hisId);
00330
00333 bool _activeFilter(int hisId);
00334
00337 bool _activeEstimate(int hisId);
00338
00340 void _activateFilter(int hisId);
00341
00343 int _getRobotsNumber();
00344
00346 int _getK_thId(int k);
00347
00348 void _doFilterTimeUpdate(map <int, vector <Pose> > &likelihoods, vector<int> &likelihoodsActive);
00349
00350 void _performMultipleRegistration(map <int, vector <Pose> > &likelihoods, vector<int> &likelihoodsActive, MultiRegSol& multiRegSol);
00351
00352 bool _performBearingOnlyMultipleRegistration(map <int, vector <Pose> > &likelihoods, vector<int> &likelihoodsActive, MultiRegCamSol* multiRegCamSol, ProbMultiRegCamSol* probMultiRegCamSol);
00353
00354
00355 };
00356
00357
00358
00359 };
00360
00361
00362 #endif
00363
00364
00365
00366