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 #ifndef CPtuHokuyo_H
00030 #define CPtuHokuyo_H
00031
00032 #include <mrpt/hwdrivers/CHokuyoURG.h>
00033
00034 #include <mrpt/utils/CFileOutputStream.h>
00035 #include "CPtuDPerception.h"
00036
00037
00038
00039 namespace mrpt
00040 {
00041 namespace hwdrivers
00042 {
00049 class HWDLLIMPEXP CPtuHokuyo;
00050 struct ThreadParams
00051 {
00052 char axis;
00053 bool start_capture;
00054 double scan_vel;
00055 CPtuHokuyo* ptu_hokuyo;
00056 };
00057
00058 class HWDLLIMPEXP CPtuHokuyo
00059 {
00060
00061 public:
00062
00063 CHokuyoURG laser;
00064 CPtuBase *ptu;
00065 std::vector<mrpt::slam::CObservation2DRangeScan> vObs;
00066
00067 double high;
00068
00069 struct my_pos
00070 {
00071 mrpt::system::TTimeStamp timeStamp;
00072 double pos;
00073 };
00074
00075 std::vector<mrpt::hwdrivers::CPtuHokuyo::my_pos> v_my_pos;
00076
00079 CPtuHokuyo() { ptu=0; high=0.095; };
00080
00083 ~CPtuHokuyo();
00084
00087 bool init(const std::string &portPtu,const std::string &portHokuyo);
00088
00098 bool scan(char &axis, const int &tWait, double &initial, double &final, const double &radPre, const int &mean, const bool &interlaced=false);
00099
00102 bool continuousScan(char &axis, const double &velocity, double &initial, double &final);
00103
00106 bool showGraphic(mrpt::slam::CSimplePointsMap *theMap=0);
00107
00110 bool saveMap2File(mrpt::slam::CSimplePointsMap &theMap, char* fname="Data.pts", const bool &colours=false);
00111
00114 bool saveVObs2File(char *fname="Data.rawlog");
00115
00118 bool saveVObsPoints2File(char *fname="Data.pts",const bool &colours=false);
00119
00122 void limit(mrpt::slam::CSimplePointsMap &theMap);
00123
00126 void setHigh(const double &newHigh) { high = newHigh; }
00127
00130 bool obtainObs( mrpt::slam::CObservation2DRangeScan & obs );
00131
00132
00133 private:
00134
00139 double saveObservation(const char &axis, const int &mean);
00140
00149 bool singleScan(const char &axis, const int &tWait, const int &movements, const double &radPre, const int &mean);
00150
00153 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs, std::vector<mrpt::slam::CObservation2DRangeScan> &vObsAux);
00154
00157 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs1, mrpt::slam::CObservation2DRangeScan &obs2, const int &mode);
00158
00161 void loadObs2PointsMap(mrpt::slam::CSimplePointsMap &theMap);
00162
00165 bool limitScan(const char &axis, double &low, double &high, mrpt::slam::CSimplePointsMap &theMap);
00166
00168 void refineVObs(const char &axis);
00169
00172 void calculateSensorPose(const char &axis, const double &pos, mrpt::slam::CObservation2DRangeScan &obs);
00173
00174
00175 };
00176
00177 }
00178
00179 }
00180
00181 #endif