ParticleMutLocCamTask.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_CAM_TASK_H_
00039 #define __PARTICLE_MUT_LOC_CAM_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
00049 #include <CommonOptions.h>
00050 #include <MultiReg.h>
00051 #include <ProbMultiReg.h>
00052 #include <MultiRegCam.h>
00053 #include <PaFilBFL.h>
00054 #include <PaFilBFLAbsRel.h>
00055 #include <PaFilBFLRelRel.h>
00056 #include <PaFilBFLRelRelBearOnly.h>
00057 #include <CommModule.h>
00058
00059
00060 using namespace MipAlgorithms;
00061 using namespace MipResources;
00062
00063
00064 namespace MipTasks{
00066
00067
00068
00075 enum FrameTypes{
00076 FIXED_FRAME,
00077 ATTACHED_FRAME,
00078 FRAME_TYPES_NUM
00079 };
00080
00081 static const char* FrameTypesNames[FRAME_TYPES_NUM] = {
00082 "fixed",
00083 "attached"
00084 };
00085
00086
00090 class ParticleMutLocCamOptions : public Options {
00091 public:
00092 DecimalOption* bLinearToll;
00093 DecimalOption* bAngularToll;
00094 IntOption* bObs0Trials;
00095 IntOption* bObs1Trials;
00096 IntOption* bMinInliersNum;
00097 DecimalOption* bPrewarpingToll;
00098
00099 DecimalOption* mLinearToll;
00100 DecimalOption* mAngularToll;
00101 DecimalOption* mMaxVisuale;
00102 DecimalOption* mDiscretizationStep;
00103
00104 DecimalOption* bmDiscretization;
00105
00106 IntOption* minObservationSize;
00107
00108 IntOption* findTarget;
00109 IntOption* targetId;
00110
00111 BoolOption* banIdActive;
00112
00113 StringOption* myFrameType;
00114 StringOption* hisFrameType;
00115 BoolOption* feedbackInMultiReg;
00116 BoolOption* bearingOnlyMeas;
00117
00118 BoolOption* saveEstimates;
00119
00120 ParticleMutLocCamOptions();
00121
00122 OptionGroupsType getGroup();
00123 string getObjectName() const {
00124 return "ParticleMutLocCamOptions";
00125 }
00126 };
00127
00128
00132 class ParticleMutLocCamSharedMem : public SharedMem{
00133 public:
00135 void initSharedMem();
00136
00140 PosiFeatures getMyProtrusions(MotionModuleTState &state);
00141
00144 void setFormation(PoseFeatures &form);
00145
00148 void setEstimates(PoseFeatures &est);
00149
00152 Pose getTarget();
00153
00156 void setTarget(Pose &t);
00157
00160 bool doMloc();
00161
00164 bool printFormation();
00165
00168 bool printFormationDetails();
00169
00172 vector<int> getAvoidParticleMutLocCamIds();
00173 };
00174
00175
00179 class ParticleMutLocCamTask : public Task{
00180 private:
00181 static const TaskPlate _plate = PMLC_TASK;
00182 static const long int _maxDurSec = 0, _maxDurUsec = 500000;
00183 static const long int _maxSamplPeriodSec = 1, _maxSamplPeriodUsec = 0;
00184 static const long int _minSamplPeriodSec = 0, _minSamplPeriodUsec = 400000;
00185
00186
00187 MobileRob* _mobileRob;
00188
00189 unsigned int _drawList;
00190
00191 ParticleMutLocCamSharedMem* _sharedMem;
00192 CommModule* _commModule;
00193
00194 FrameTypes _myFrameType;
00195 FrameTypes _hisFrameType;
00196
00197
00198 ParticleMutLocCamOptions _options;
00199
00200
00201 MultiReg* _multiReg;
00202 ProbMultiReg* _probMultiReg;
00203 map<int,MutLocFilter*> _filter;
00204 vector<int> _activatedFilters;
00205
00206
00207 bool _domultireg;
00208 bool _dofilter;
00209 vector<int> ignoreIds;
00210 bool _printFormation;
00211 bool _printFormationDetails;
00212
00213
00214 int _myId;
00215 Pose _myPose;
00216 PosiFeatures _myPosiFeatures;
00217 Observation _myObservation;
00218
00219
00220 map<int,PosiFeatures> _othersPosiFeatures;
00221 vector<int> _othersPosiFeaturesNums;
00222 map<int,Pose> _othersPose;
00223 vector<int> _othersPoseNums;
00224
00225
00226 map<int,Pose> _estimates;
00227 vector<int> _activatedEstimates;
00228 map<int,Timer> _timeFromLastMessage;
00229 PoseFeatures _formation;
00230
00231 fstream _savefile;
00232
00233 public:
00235 ParticleMutLocCamTask(ResourcePointers resources,int argc, const char* argv[]);
00236
00238 ~ParticleMutLocCamTask();
00239
00242 TaskPlate getPlate() const {
00243 return _plate;
00244 }
00247 Time getMaxDuration(){
00248 return Time(_maxDurSec,_maxDurUsec);
00249 }
00252 Time getMaxSamplPeriod(){
00253 return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
00254 }
00257 Time getMinSamplPeriod(){
00258 return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
00259 }
00262 TaskOutputs run();
00263
00264 private:
00265
00267 void _readSharedMemory();
00268
00270 void _comunicate();
00271
00273 void _draw();
00274
00276 void _writeSharedMem();
00277
00278 void _computeFormation();
00279
00282 void _insertValidObservations(vector<Observation> &allObservations);
00283
00287 void _completeMutLocFilInput(MutLocFilInput &fStepIn, int hisId);
00288
00291 bool _activeFilter(int hisId);
00292
00295 bool _activeEstimate(int hisId);
00296
00298 void _activateFilter(int hisId);
00299 };
00300
00301
00302
00303 };
00304
00305
00306 #endif
00307
00308
00309
00310