00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://www.mrpt.org/ | 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 * \ingroup mrpt_hwdrivers_grp 00045 */ 00046 00047 class HWDRIVERS_IMPEXP CPtuHokuyo; 00048 00049 struct ThreadParams 00050 { 00051 char axis; 00052 bool start_capture; 00053 double scan_vel; // Velocity of continuous scan 00054 CPtuHokuyo* ptu_hokuyo; 00055 }; 00056 00057 class HWDRIVERS_IMPEXP CPtuHokuyo : public CGenericSensor 00058 { 00059 00060 DEFINE_GENERIC_SENSOR(CPtuHokuyo) 00061 00062 protected: 00063 00064 std::string m_ptu_port; 00065 char m_axis; 00066 double m_velocity, m_initial, m_final, m_hokuyo_frec; 00067 00068 /** 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) 00069 * \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value. 00070 */ 00071 void loadConfig_sensorSpecific( 00072 const mrpt::utils::CConfigFileBase &configSource, 00073 const std::string §ion ); 00074 00075 public: 00076 00077 CHokuyoURG laser; 00078 CPtuBase *ptu; 00079 00080 /** Specify type of ptu. Current options are: 00081 * m_ptu_type = 0 => CPtuDPerception 00082 * m_ptu_type = 1 => CPtuMicos 00083 */ 00084 int m_ptu_type; 00085 00086 std::vector<mrpt::slam::CObservation2DRangeScan> vObs; 00087 00088 // High between ptu tilt axis and hokuyo laser scan 00089 double high; 00090 00091 struct my_pos 00092 { 00093 mrpt::system::TTimeStamp timeStamp; 00094 double pos; 00095 }; 00096 00097 std::vector<mrpt::hwdrivers::CPtuHokuyo::my_pos> v_my_pos; 00098 std::vector<double> v_ptu_pos, v_ptu_time; 00099 00100 00101 /** Default constructor */ 00102 00103 CPtuHokuyo(); 00104 00105 /** Destructor, delete observations of the vector */ 00106 00107 ~CPtuHokuyo(); 00108 00109 /** Initialization of laser and ptu */ 00110 00111 bool init(const std::string &portPtu, const std::string &portHokuyo); 00112 00113 /** Performs a complete scan 00114 * \param <axis> Pan or Till 00115 * \param <tWait> Wait time betwen commands 00116 * \param <initial> initial position 00117 * \param <final> final position 00118 * \param <radPre> radians precision for the scan 00119 * \param <interlaced> if interlaced==true performs a double sweep 00120 */ 00121 00122 bool scan(char &axis, const int &tWait, double &initial, double &final, const double &radPre, const int &mean, const bool &interlaced=false); 00123 00124 /** Performs a continuous scan */ 00125 00126 bool continuousScan(char &axis, const double &velocity, double &initial, double &final); 00127 00128 /** Show a graphic with the points obtained from the scan or a map*/ 00129 //bool showGraphic(mrpt::slam::CSimplePointsMap *theMap=0); 00130 00131 /** Save a simple points map into a simple file (if colours==true save points with a color) */ 00132 //bool saveMap2File(mrpt::slam::CSimplePointsMap &theMap, char* fname="Data.pts", const bool &colours=false); 00133 00134 /** Save vector of observations in a CFileOutputStream file */ 00135 00136 bool saveVObs2File(char *fname="Data.rawlog"); 00137 00138 /** Save vector points of observations into a simple file */ 00139 00140 bool saveVObsPoints2File(char *fname="Data.pts",const bool &colours=false); 00141 00142 /** Save pitchs and raw distances of all scans */ 00143 00144 bool savePitchAndDistances2File(); 00145 00146 /** Method for limit map points obtained from a scan */ 00147 //void limit(mrpt::slam::CSimplePointsMap &theMap); 00148 00149 /** Set high between ptu tilt axis and hokuyo laser scan */ 00150 00151 void setHigh(const double &newHigh) { high = newHigh; } 00152 00153 /** Obtain a observation from the laser */ 00154 00155 bool obtainObs( mrpt::slam::CObservation2DRangeScan & obs ); 00156 00157 /** This method can or cannot be implemented in the derived class, depending on the need for it. 00158 * \exception This method must throw an exception with a descriptive message if some critical error is found. 00159 */ 00160 void initialize(); 00161 00162 /** This method will be invoked at a minimum rate of "process_rate" (Hz) 00163 * \exception This method must throw an exception with a descriptive message if some critical error is found. 00164 */ 00165 void doProcess(); 00166 00167 00168 private: 00169 00170 /** Save a observation from the laser into a vector of 00171 * observations, calculating sensor position 00172 */ 00173 00174 double saveObservation(const char &axis, const int &mean); 00175 00176 /** Performs a simple scan 00177 * \param <axis> Pan or Till 00178 * \param <tWait> Wait time betwen commands 00179 * \param <movements> number total of movements 00180 * \param <radPre> radians precision for the scan 00181 * \param <vObs> reference to obsevations vector for save the observation 00182 */ 00183 00184 bool singleScan(const char &axis, const int &tWait, const int &movements, const double &radPre, const int &mean); 00185 00186 /** Calculate minimum lenght of scan vectors */ 00187 00188 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs, std::vector<mrpt::slam::CObservation2DRangeScan> &vObsAux); 00189 00190 /** Calculate minimum lenght of 2 scan vectors */ 00191 00192 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs1, mrpt::slam::CObservation2DRangeScan &obs2, const int &mode); 00193 00194 /** Load observations in a points map */ 00195 //void loadObs2PointsMap(mrpt::slam::CSimplePointsMap &theMap); 00196 00197 /** Limit the valid position of scan points */ 00198 //bool limitScan(const char &axis, double &low, double &high, mrpt::slam::CSimplePointsMap &theMap); 00199 00200 /** Refine the observations obtains from a continuous scan */ 00201 void refineVObs(const char &axis); 00202 00203 /** Calculate the sensor pose depending teh axis of movements and the ptu position */ 00204 00205 void calculateSensorPose(const char &axis, const double &pos, mrpt::slam::CObservation2DRangeScan &obs); 00206 00207 /** Obtain position of observations between first and second position in m_my_pos map */ 00208 00209 int obsPosition(); 00210 00211 00212 }; // End of class 00213 00214 } // End of namespace 00215 00216 } // End of namespace 00217 00218 #endif
| Page generated by Doxygen 1.7.5 for MRPT 0.9.5 SVN: at Thu Oct 13 21:25:36 UTC 2011 |