00001 /* +---------------------------------------------------------------------------+ 00002 | The Mobile Robot Programming Toolkit (MRPT) C++ library | 00003 | | 00004 | http://mrpt.sourceforge.net/ | 00005 | | 00006 | Copyright (C) 2005-2009 University of Malaga | 00007 | | 00008 | This software was written by the Perception and Robotics | 00009 | research group, 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 public: 00068 00069 CHokuyoURG laser; 00070 CPtuBase *ptu; 00071 00072 /** Specify type of ptu. Current options are: 00073 * m_ptu_type = 0 => CPtuDPerception 00074 * m_ptu_type = 1 => CPtuMicos 00075 */ 00076 int m_ptu_type; 00077 00078 std::vector<mrpt::slam::CObservation2DRangeScan> vObs; 00079 00080 // High between ptu tilt axis and hokuyo laser scan 00081 double high; 00082 00083 struct my_pos 00084 { 00085 mrpt::system::TTimeStamp timeStamp; 00086 double pos; 00087 }; 00088 00089 std::vector<mrpt::hwdrivers::CPtuHokuyo::my_pos> v_my_pos; 00090 std::vector<double> v_ptu_pos, v_ptu_time; 00091 00092 00093 /** Default constructor */ 00094 00095 CPtuHokuyo(); 00096 00097 /** Destructor, delete observations of the vector */ 00098 00099 ~CPtuHokuyo(); 00100 00101 /** Initialization of laser and ptu */ 00102 00103 bool init(const std::string &portPtu, const std::string &portHokuyo); 00104 00105 /** Performs a complete scan 00106 * \param <axis> Pan or Till 00107 * \param <tWait> Wait time betwen commands 00108 * \param <initial> initial position 00109 * \param <final> final position 00110 * \param <radPre> radians precision for the scan 00111 * \param <interlaced> if interlaced==true performs a double sweep 00112 */ 00113 00114 bool scan(char &axis, const int &tWait, double &initial, double &final, const double &radPre, const int &mean, const bool &interlaced=false); 00115 00116 /** Performs a continuous scan */ 00117 00118 bool continuousScan(char &axis, const double &velocity, double &initial, double &final); 00119 00120 /** Show a graphic with the points obtained from the scan or a map*/ 00121 00122 bool showGraphic(mrpt::slam::CSimplePointsMap *theMap=0); 00123 00124 /** Save a simple points map into a simple file (if colours==true save points with a color) */ 00125 00126 bool saveMap2File(mrpt::slam::CSimplePointsMap &theMap, char* fname="Data.pts", const bool &colours=false); 00127 00128 /** Save vector of observations in a CFileOutputStream file */ 00129 00130 bool saveVObs2File(char *fname="Data.rawlog"); 00131 00132 /** Save vector points of observations into a simple file */ 00133 00134 bool saveVObsPoints2File(char *fname="Data.pts",const bool &colours=false); 00135 00136 /** Method for limit map points obtained from a scan */ 00137 00138 void limit(mrpt::slam::CSimplePointsMap &theMap); 00139 00140 /** Set high between ptu tilt axis and hokuyo laser scan */ 00141 00142 void setHigh(const double &newHigh) { high = newHigh; } 00143 00144 /** Obtain a observation from the laser */ 00145 00146 bool obtainObs( mrpt::slam::CObservation2DRangeScan & obs ); 00147 00148 /** 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) 00149 * \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value. 00150 */ 00151 void loadConfig( 00152 const mrpt::utils::CConfigFileBase &configSource, 00153 const std::string §ion ); 00154 00155 /** This method can or cannot be implemented in the derived class, depending on the need for it. 00156 * \exception This method must throw an exception with a descriptive message if some critical error is found. 00157 */ 00158 void initialize(); 00159 00160 /** This method will be invoked at a minimum rate of "process_rate" (Hz) 00161 * \exception This method must throw an exception with a descriptive message if some critical error is found. 00162 */ 00163 void doProcess(); 00164 00165 00166 private: 00167 00168 /** Save a observation from the laser into a vector of 00169 * observations, calculating sensor position 00170 */ 00171 00172 double saveObservation(const char &axis, const int &mean); 00173 00174 /** Performs a simple scan 00175 * \param <axis> Pan or Till 00176 * \param <tWait> Wait time betwen commands 00177 * \param <movements> number total of movements 00178 * \param <radPre> radians precision for the scan 00179 * \param <vObs> reference to obsevations vector for save the observation 00180 */ 00181 00182 bool singleScan(const char &axis, const int &tWait, const int &movements, const double &radPre, const int &mean); 00183 00184 /** Calculate minimum lenght of scan vectors */ 00185 00186 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs, std::vector<mrpt::slam::CObservation2DRangeScan> &vObsAux); 00187 00188 /** Calculate minimum lenght of 2 scan vectors */ 00189 00190 int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs1, mrpt::slam::CObservation2DRangeScan &obs2, const int &mode); 00191 00192 /** Load observations in a points map */ 00193 00194 void loadObs2PointsMap(mrpt::slam::CSimplePointsMap &theMap); 00195 00196 /** Limit the valid position of scan points */ 00197 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.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:27:43 EDT 2009 |