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 : public CGenericSensor
00059 {
00060
00061 DEFINE_GENERIC_SENSOR(CPtuHokuyo)
00062
00063 protected:
00064
00065 std::string m_ptu_port;
00066 char m_axis;
00067 double m_velocity, m_initial, m_final, m_hokuyo_frec;
00068
00069 public:
00070
00071 CHokuyoURG laser;
00072 CPtuBase *ptu;
00073 std::vector<mrpt::slam::CObservation2DRangeScan> vObs;
00074
00075
00076 double high;
00077
00078 struct my_pos
00079 {
00080 mrpt::system::TTimeStamp timeStamp;
00081 double pos;
00082 };
00083
00084 std::vector<mrpt::hwdrivers::CPtuHokuyo::my_pos> v_my_pos;
00085 std::vector<double> v_ptu_pos, v_ptu_time;
00086
00087
00090 CPtuHokuyo();
00091
00094 ~CPtuHokuyo();
00095
00098 bool init(const std::string &portPtu, const std::string &portHokuyo);
00099
00109 bool scan(char &axis, const int &tWait, double &initial, double &final, const double &radPre, const int &mean, const bool &interlaced=false);
00110
00113 bool continuousScan(char &axis, const double &velocity, double &initial, double &final);
00114
00117 bool showGraphic(mrpt::slam::CSimplePointsMap *theMap=0);
00118
00121 bool saveMap2File(mrpt::slam::CSimplePointsMap &theMap, char* fname="Data.pts", const bool &colours=false);
00122
00125 bool saveVObs2File(char *fname="Data.rawlog");
00126
00129 bool saveVObsPoints2File(char *fname="Data.pts",const bool &colours=false);
00130
00133 void limit(mrpt::slam::CSimplePointsMap &theMap);
00134
00137 void setHigh(const double &newHigh) { high = newHigh; }
00138
00141 bool obtainObs( mrpt::slam::CObservation2DRangeScan & obs );
00142
00146 void loadConfig(
00147 const mrpt::utils::CConfigFileBase &configSource,
00148 const std::string §ion );
00149
00153 void initialize();
00154
00158 void doProcess();
00159
00160
00161 private:
00162
00167 double saveObservation(const char &axis, const int &mean);
00168
00177 bool singleScan(const char &axis, const int &tWait, const int &movements, const double &radPre, const int &mean);
00178
00181 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs, std::vector<mrpt::slam::CObservation2DRangeScan> &vObsAux);
00182
00185 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs1, mrpt::slam::CObservation2DRangeScan &obs2, const int &mode);
00186
00189 void loadObs2PointsMap(mrpt::slam::CSimplePointsMap &theMap);
00190
00193 bool limitScan(const char &axis, double &low, double &high, mrpt::slam::CSimplePointsMap &theMap);
00194
00196 void refineVObs(const char &axis);
00197
00200 void calculateSensorPose(const char &axis, const double &pos, mrpt::slam::CObservation2DRangeScan &obs);
00201
00204 int obsPosition();
00205
00206
00207 };
00208
00209 }
00210
00211 }
00212
00213 #endif