MutLocTask.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
00029
00030
00035
00036
00037
00038 #ifndef __MUT_LOC_TASK_H_
00039 #define __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
00049 #include <CommonOptions.h>
00050 #include <MultiReg.h>
00051 #include <DAEKF.h>
00052 #include <PaFil.h>
00053 #include <CommModule.h>
00054
00055
00056 using namespace MipAlgorithms;
00057 using namespace MipResources;
00058
00059 namespace MipTasks{
00060
00062
00063
00064
00068 class MutLocOptions : public Options {
00069 public:
00070 DecimalOption* bLinearToll;
00071 DecimalOption* bAngularToll;
00072 IntOption* bObs0Trials;
00073 IntOption* bObs1Trials;
00074 IntOption* bMinInliersNum;
00075 DecimalOption* bPrewarpingToll;
00076
00077 DecimalOption* mLinearToll;
00078 DecimalOption* mAngularToll;
00079 DecimalOption* mMaxVisuale;
00080 DecimalOption* mDiscretizationStep;
00081
00082 DecimalOption* bmDiscretization;
00083
00084 IntOption* filterType;
00085
00086 IntOption* minObservationSize;
00087
00088 IntOption* findTarget;
00089 IntOption* targetId;
00090
00091 BoolOption* banIdActive;
00092
00093 BoolOption* saveEstimates;
00094
00095 MutLocOptions();
00096
00097 OptionGroupsType getGroup();
00098 string getObjectName() const {
00099 return "MutLocOptions";
00100 }
00101 };
00102
00103
00107 class MutLocSharedMem : public SharedMem{
00108 public:
00110 void initSharedMem();
00111
00115 PosiFeatures getMyProtrusions(MotionModuleTState &state);
00116
00119 void setFormation(PoseFeatures &form);
00120
00123 void setEstimates(PoseFeatures &est);
00124
00127 Pose getTarget();
00128
00131 void setTarget(Pose &t);
00132
00135 bool doMloc();
00136
00139 bool printFormation();
00140
00143 bool printFormationDetails();
00144
00147 vector<int> getAvoidMutLocIds();
00148 };
00149
00150
00154 class MutLocTask : public Task{
00155 private:
00156 static const TaskPlate _plate = MTL_TASK;
00157 static const long int _maxDurSec = 0, _maxDurUsec = 500000;
00158 static const long int _maxSamplPeriodSec = 1, _maxSamplPeriodUsec = 0;
00159 static const long int _minSamplPeriodSec = 0, _minSamplPeriodUsec = 400000;
00160
00161 MobileRob* _mobileRob;
00162 Position robPosition;
00163 Pose robPose;
00164
00165 DraWin* _draWin;
00166 unsigned int _drawList;
00167 bool dwOn;
00168
00169 MutLocSharedMem* _sharedMem;
00170
00171 CommModule* _commModule;
00172
00173 map<int,Pose> estimates;
00174 vector<int> activatedEstimates;
00175 map<int,Timer> timeFromLastMessage;
00176
00177 MultiReg *multiReg;
00178 map<int,MutLocFilter*> filter;
00179 vector<int> activatedFilters;
00180
00181 vector<int> ignoreIds;
00182
00183 bool domultireg;
00184 bool dofilter;
00185
00186 bool fifoactive;
00187 stringstream fifoName;
00188 FifoTerm * fifo;
00189
00190 map<int,PosiFeatures> othersPosiFeatures;
00191 map<int,Pose> othersPose;
00192 vector<int> othersPosiFeaturesNums;
00193 vector<int> othersPoseNums;
00194
00195 int myId;
00196 Pose myPose;
00197 PosiFeatures myPosiFeatures;
00198 Observation myObservation;
00199
00200 PoseFeatures formation;
00201
00202 MutLocOptions options;
00203
00204 fstream savefile;
00205
00206 public:
00208 MutLocTask(ResourcePointers resources,int argc, const char* argv[]);
00209
00211 ~MutLocTask();
00212
00215 TaskPlate getPlate() const {
00216 return _plate;
00217 }
00220 Time getMaxDuration(){
00221 return Time(_maxDurSec,_maxDurUsec);
00222 }
00225 Time getMaxSamplPeriod(){
00226 return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
00227 }
00230 Time getMinSamplPeriod(){
00231 return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
00232 }
00235 TaskOutputs run();
00236
00237 private:
00238
00240 void comunicate();
00241
00243 void draw();
00244
00246 void fifoSend();
00247
00249 void writeSharedMem();
00250
00251 void computeFormation();
00252
00255 void insertValidObservations(vector<Observation> &allObservations);
00256
00260 void completeMutLocFilInput(MutLocFilInput &fStepIn, int hisId);
00261
00264 bool activeFilter(int hisId);
00265
00268 bool activeEstimate(int hisId);
00269 };
00270
00271
00272
00273 };
00274
00275
00276 #endif
00277
00278
00279
00280
00281