00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2010 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 HWDLLIMPEXP 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 HWDLLIMPEXP 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 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 00133 bool saveMap2File(mrpt::slam::CSimplePointsMap &theMap, char* fname="Data.pts", const bool &colours=false); 00134 00135 /** Save vector of observations in a CFileOutputStream file */ 00136 00137 bool saveVObs2File(char *fname="Data.rawlog"); 00138 00139 /** Save vector points of observations into a simple file */ 00140 00141 bool saveVObsPoints2File(char *fname="Data.pts",const bool &colours=false); 00142 00143 /** Save pitchs and raw distances of all scans */ 00144 00145 bool savePitchAndDistances2File(); 00146 00147 /** Method for limit map points obtained from a scan */ 00148 00149 void limit(mrpt::slam::CSimplePointsMap &theMap); 00150 00151 /** Set high between ptu tilt axis and hokuyo laser scan */ 00152 00153 void setHigh(const double &newHigh) { high = newHigh; } 00154 00155 /** Obtain a observation from the laser */ 00156 00157 bool obtainObs( mrpt::slam::CObservation2DRangeScan & obs ); 00158 00159 /** This method can or cannot be implemented in the derived class, depending on the need for it. 00160 * \exception This method must throw an exception with a descriptive message if some critical error is found. 00161 */ 00162 void initialize(); 00163 00164 /** This method will be invoked at a minimum rate of "process_rate" (Hz) 00165 * \exception This method must throw an exception with a descriptive message if some critical error is found. 00166 */ 00167 void doProcess(); 00168 00169 00170 private: 00171 00172 /** Save a observation from the laser into a vector of 00173 * observations, calculating sensor position 00174 */ 00175 00176 double saveObservation(const char &axis, const int &mean); 00177 00178 /** Performs a simple scan 00179 * \param <axis> Pan or Till 00180 * \param <tWait> Wait time betwen commands 00181 * \param <movements> number total of movements 00182 * \param <radPre> radians precision for the scan 00183 * \param <vObs> reference to obsevations vector for save the observation 00184 */ 00185 00186 bool singleScan(const char &axis, const int &tWait, const int &movements, const double &radPre, const int &mean); 00187 00188 /** Calculate minimum lenght of scan vectors */ 00189 00190 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs, std::vector<mrpt::slam::CObservation2DRangeScan> &vObsAux); 00191 00192 /** Calculate minimum lenght of 2 scan vectors */ 00193 00194 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs1, mrpt::slam::CObservation2DRangeScan &obs2, const int &mode); 00195 00196 /** Load observations in a points map */ 00197 00198 void loadObs2PointsMap(mrpt::slam::CSimplePointsMap &theMap); 00199 00200 /** Limit the valid position of scan points */ 00201 00202 bool limitScan(const char &axis, double &low, double &high, mrpt::slam::CSimplePointsMap &theMap); 00203 00204 /** Refine the observations obtains from a continuous scan */ 00205 void refineVObs(const char &axis); 00206 00207 /** Calculate the sensor pose depending teh axis of movements and the ptu position */ 00208 00209 void calculateSensorPose(const char &axis, const double &pos, mrpt::slam::CObservation2DRangeScan &obs); 00210 00211 /** Obtain position of observations between first and second position in m_my_pos map */ 00212 00213 int obsPosition(); 00214 00215 00216 }; // End of class 00217 00218 } // End of namespace 00219 00220 } // End of namespace 00221 00222 #endif
| Page generated by Doxygen 1.6.2 for MRPT 0.8.1 SVN:exported at Mon Feb 15 22:01:07 UTC 2010 |