00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2011 University of Malaga | 00007 | | 00008 | This software was written by the Machine Perception and Intelligent | 00009 | Robotics Lab, University of Malaga (Spain). | 00010 | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> | 00011 | | 00012 | This file is part of the MRPT project. | 00013 | | 00014 | MRPT is free software: you can redistribute it and/or modify | 00015 | it under the terms of the GNU General Public License as published by | 00016 | the Free Software Foundation, either version 3 of the License, or | 00017 | (at your option) any later version. | 00018 | | 00019 | MRPT is distributed in the hope that it will be useful, | 00020 | but WITHOUT ANY WARRANTY; without even the implied warranty of | 00021 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | 00022 | GNU General Public License for more details. | 00023 | | 00024 | You should have received a copy of the GNU General Public License | 00025 | along with MRPT. If not, see <http://www.gnu.org/licenses/>. | 00026 | | 00027 +---------------------------------------------------------------------------+ */ 00028 00029 #ifndef CPtuHokuyo_H 00030 #define CPtuHokuyo_H 00031 00032 #include <mrpt/hwdrivers/CHokuyoURG.h> 00033 #include <mrpt/utils/CFileOutputStream.h> 00034 #include "CPtuDPerception.h" 00035 00036 namespace mrpt 00037 { 00038 namespace hwdrivers 00039 { 00040 /** The objetive of this class is to coordinate PTU movements and 00041 * Hokuyo scans, adding the posibility of save the points earned 00042 * in several different formats, limit valids points and view 00043 * them on a grahic. 00044 */ 00045 00046 class HWDRIVERS_IMPEXP CPtuHokuyo; 00047 00048 struct ThreadParams 00049 { 00050 char axis; 00051 bool start_capture; 00052 double scan_vel; // Velocity of continuous scan 00053 CPtuHokuyo* ptu_hokuyo; 00054 }; 00055 00056 class HWDRIVERS_IMPEXP CPtuHokuyo : public CGenericSensor 00057 { 00058 00059 DEFINE_GENERIC_SENSOR(CPtuHokuyo) 00060 00061 protected: 00062 00063 std::string m_ptu_port; 00064 char m_axis; 00065 double m_velocity, m_initial, m_final, m_hokuyo_frec; 00066 00067 /** Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes) 00068 * \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value. 00069 */ 00070 void loadConfig_sensorSpecific( 00071 const mrpt::utils::CConfigFileBase &configSource, 00072 const std::string §ion ); 00073 00074 public: 00075 00076 CHokuyoURG laser; 00077 CPtuBase *ptu; 00078 00079 /** Specify type of ptu. Current options are: 00080 * m_ptu_type = 0 => CPtuDPerception 00081 * m_ptu_type = 1 => CPtuMicos 00082 */ 00083 int m_ptu_type; 00084 00085 std::vector<mrpt::slam::CObservation2DRangeScan> vObs; 00086 00087 // High between ptu tilt axis and hokuyo laser scan 00088 double high; 00089 00090 struct my_pos 00091 { 00092 mrpt::system::TTimeStamp timeStamp; 00093 double pos; 00094 }; 00095 00096 std::vector<mrpt::hwdrivers::CPtuHokuyo::my_pos> v_my_pos; 00097 std::vector<double> v_ptu_pos, v_ptu_time; 00098 00099 00100 /** Default constructor */ 00101 00102 CPtuHokuyo(); 00103 00104 /** Destructor, delete observations of the vector */ 00105 00106 ~CPtuHokuyo(); 00107 00108 /** Initialization of laser and ptu */ 00109 00110 bool init(const std::string &portPtu, const std::string &portHokuyo); 00111 00112 /** Performs a complete scan 00113 * \param <axis> Pan or Till 00114 * \param <tWait> Wait time betwen commands 00115 * \param <initial> initial position 00116 * \param <final> final position 00117 * \param <radPre> radians precision for the scan 00118 * \param <interlaced> if interlaced==true performs a double sweep 00119 */ 00120 00121 bool scan(char &axis, const int &tWait, double &initial, double &final, const double &radPre, const int &mean, const bool &interlaced=false); 00122 00123 /** Performs a continuous scan */ 00124 00125 bool continuousScan(char &axis, const double &velocity, double &initial, double &final); 00126 00127 /** Show a graphic with the points obtained from the scan or a map*/ 00128 //bool showGraphic(mrpt::slam::CSimplePointsMap *theMap=0); 00129 00130 /** Save a simple points map into a simple file (if colours==true save points with a color) */ 00131 //bool saveMap2File(mrpt::slam::CSimplePointsMap &theMap, char* fname="Data.pts", const bool &colours=false); 00132 00133 /** Save vector of observations in a CFileOutputStream file */ 00134 00135 bool saveVObs2File(char *fname="Data.rawlog"); 00136 00137 /** Save vector points of observations into a simple file */ 00138 00139 bool saveVObsPoints2File(char *fname="Data.pts",const bool &colours=false); 00140 00141 /** Save pitchs and raw distances of all scans */ 00142 00143 bool savePitchAndDistances2File(); 00144 00145 /** Method for limit map points obtained from a scan */ 00146 //void limit(mrpt::slam::CSimplePointsMap &theMap); 00147 00148 /** Set high between ptu tilt axis and hokuyo laser scan */ 00149 00150 void setHigh(const double &newHigh) { high = newHigh; } 00151 00152 /** Obtain a observation from the laser */ 00153 00154 bool obtainObs( mrpt::slam::CObservation2DRangeScan & obs ); 00155 00156 /** This method can or cannot be implemented in the derived class, depending on the need for it. 00157 * \exception This method must throw an exception with a descriptive message if some critical error is found. 00158 */ 00159 void initialize(); 00160 00161 /** This method will be invoked at a minimum rate of "process_rate" (Hz) 00162 * \exception This method must throw an exception with a descriptive message if some critical error is found. 00163 */ 00164 void doProcess(); 00165 00166 00167 private: 00168 00169 /** Save a observation from the laser into a vector of 00170 * observations, calculating sensor position 00171 */ 00172 00173 double saveObservation(const char &axis, const int &mean); 00174 00175 /** Performs a simple scan 00176 * \param <axis> Pan or Till 00177 * \param <tWait> Wait time betwen commands 00178 * \param <movements> number total of movements 00179 * \param <radPre> radians precision for the scan 00180 * \param <vObs> reference to obsevations vector for save the observation 00181 */ 00182 00183 bool singleScan(const char &axis, const int &tWait, const int &movements, const double &radPre, const int &mean); 00184 00185 /** Calculate minimum lenght of scan vectors */ 00186 00187 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs, std::vector<mrpt::slam::CObservation2DRangeScan> &vObsAux); 00188 00189 /** Calculate minimum lenght of 2 scan vectors */ 00190 00191 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs1, mrpt::slam::CObservation2DRangeScan &obs2, const int &mode); 00192 00193 /** Load observations in a points map */ 00194 //void loadObs2PointsMap(mrpt::slam::CSimplePointsMap &theMap); 00195 00196 /** Limit the valid position of scan points */ 00197 //bool limitScan(const char &axis, double &low, double &high, mrpt::slam::CSimplePointsMap &theMap); 00198 00199 /** Refine the observations obtains from a continuous scan */ 00200 void refineVObs(const char &axis); 00201 00202 /** Calculate the sensor pose depending teh axis of movements and the ptu position */ 00203 00204 void calculateSensorPose(const char &axis, const double &pos, mrpt::slam::CObservation2DRangeScan &obs); 00205 00206 /** Obtain position of observations between first and second position in m_my_pos map */ 00207 00208 int obsPosition(); 00209 00210 00211 }; // End of class 00212 00213 } // End of namespace 00214 00215 } // End of namespace 00216 00217 #endif
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011 |