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
00027
00028
00029
00030
00031
00032
00033
00034 #ifndef SFMAPPER_H
00035 #define SFMAPPER_H
00036
00037 #include "export.h"
00038 #include "scanstudio/mapper.h"
00039
00040 extern const long MAPPER_MAX_ROBOTS;
00041
00042 class SfLaserScan;
00043 class asyncLaserScanProcessor;
00044
00045 enum updateType { NOUPDATE=0, SCANSTUDIO=1 };
00046
00047 class SfMapper : public SfUTask
00048 {
00049 public:
00050 SfMapper();
00051 void process();
00052 void reset();
00053 std::list<SfLaserScan*>* setOfScans;
00054 ulong nextScanId;
00055 struct MapperHandle* MapperHandle;
00056 updateType updateMethod;
00057 ArSickLogger* logger;
00058 int robot_id;
00059 int transmitData;
00060
00061 void resetScanStudio();
00062
00063 void setMinAngle(double angle)
00064 { move_da = angle; }
00065 void setMinDistance(double dist)
00066 { move_ds = dist; }
00067 void setRobotId(int id)
00068 { robot_id = id; }
00069 void startTransmitData()
00070 { transmitData = 1; }
00071 void stopTransmitData()
00072 { transmitData = 0; }
00073
00074 private:
00075 asyncLaserScanProcessor* scanProcessor;
00076 ArPose lastPose;
00077
00078 ArMutex processingMutex;
00079
00080 double move_ds, move_da;
00081
00082
00083 struct PoseProb* PoseProb;
00084 short PoseProbType;
00085 double PoseGridXRes, PoseGridYRes, PoseGridARes;
00086 long PoseSamplesNum;
00087 ScanMatcher* MapperScanMatcher;
00088 double MapperMinOverlap;
00089 short MapperChainLen;
00090 double MapperMergeRadius;
00091 int MapperRectilinear;
00092 double MapperCloseLoopMinProb;
00093 int MapperCPEMethod;
00094
00095 static bool mapper_closeloopcallback();
00096 static void mapper_redrawcallback(struct Scan **scans, long numScans,
00097 struct PoseGauss (*robotPos)[MAPPER_MAX_ROBOTS], bool all);
00098 };
00099
00100
00101 class asyncLaserScanProcessor : public ArRecurrentTask
00102 {
00103 public:
00104 asyncLaserScanProcessor(SfMapper* m);
00105 void task();
00106
00107 private:
00108 SfMapper* mapper;
00109 };
00110
00111
00112 #endif // SFMAPPER_H