Main MRPT website > C++ reference
MRPT logo
CPtuHokuyo.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 
29 #ifndef CPtuHokuyo_H
30 #define CPtuHokuyo_H
31 
32 #include <mrpt/hwdrivers/CHokuyoURG.h>
33 #include <mrpt/utils/CFileOutputStream.h>
34 #include "CPtuDPerception.h"
35 
36 namespace mrpt
37 {
38  namespace hwdrivers
39  {
40  /** The objetive of this class is to coordinate PTU movements and
41  * Hokuyo scans, adding the posibility of save the points earned
42  * in several different formats, limit valids points and view
43  * them on a grahic.
44  * \ingroup mrpt_hwdrivers_grp
45  */
46 
47  class HWDRIVERS_IMPEXP CPtuHokuyo;
48 
49  struct ThreadParams
50  {
51  char axis;
52  bool start_capture;
53  double scan_vel; // Velocity of continuous scan
54  CPtuHokuyo* ptu_hokuyo;
55  };
56 
57  class HWDRIVERS_IMPEXP CPtuHokuyo : public CGenericSensor
58  {
59 
60  DEFINE_GENERIC_SENSOR(CPtuHokuyo)
61 
62  protected:
63 
64  std::string m_ptu_port;
65  char m_axis;
66  double m_velocity, m_initial, m_final, m_hokuyo_frec;
67 
68  /** 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)
69  * \exception This method must throw an exception with a descriptive message if some critical parameter is missing or has an invalid value.
70  */
71  void loadConfig_sensorSpecific(
72  const mrpt::utils::CConfigFileBase &configSource,
73  const std::string &section );
74 
75  public:
76 
77  CHokuyoURG laser;
78  CPtuBase *ptu;
79 
80  /** Specify type of ptu. Current options are:
81  * m_ptu_type = 0 => CPtuDPerception
82  * m_ptu_type = 1 => CPtuMicos
83  */
84  int m_ptu_type;
85 
86  std::vector<mrpt::slam::CObservation2DRangeScan> vObs;
87 
88  // High between ptu tilt axis and hokuyo laser scan
89  double high;
90 
91  struct my_pos
92  {
93  mrpt::system::TTimeStamp timeStamp;
94  double pos;
95  };
96 
97  std::vector<mrpt::hwdrivers::CPtuHokuyo::my_pos> v_my_pos;
98  std::vector<double> v_ptu_pos, v_ptu_time;
99 
100 
101  /** Default constructor */
102 
103  CPtuHokuyo();
104 
105  /** Destructor, delete observations of the vector */
106 
107  ~CPtuHokuyo();
108 
109  /** Initialization of laser and ptu */
110 
111  bool init(const std::string &portPtu, const std::string &portHokuyo);
112 
113  /** Performs a complete scan
114  * \param <axis> Pan or Till
115  * \param <tWait> Wait time betwen commands
116  * \param <initial> initial position
117  * \param <final> final position
118  * \param <radPre> radians precision for the scan
119  * \param <interlaced> if interlaced==true performs a double sweep
120  */
121 
122  bool scan(char &axis, const int &tWait, double &initial, double &final, const double &radPre, const int &mean, const bool &interlaced=false);
123 
124  /** Performs a continuous scan */
125 
126  bool continuousScan(char &axis, const double &velocity, double &initial, double &final);
127 
128  /** Show a graphic with the points obtained from the scan or a map*/
129  //bool showGraphic(mrpt::slam::CSimplePointsMap *theMap=0);
130 
131  /** Save a simple points map into a simple file (if colours==true save points with a color) */
132  //bool saveMap2File(mrpt::slam::CSimplePointsMap &theMap, char* fname="Data.pts", const bool &colours=false);
133 
134  /** Save vector of observations in a CFileOutputStream file */
135 
136  bool saveVObs2File(char *fname="Data.rawlog");
137 
138  /** Save vector points of observations into a simple file */
139 
140  bool saveVObsPoints2File(char *fname="Data.pts",const bool &colours=false);
141 
142  /** Save pitchs and raw distances of all scans */
143 
144  bool savePitchAndDistances2File();
145 
146  /** Method for limit map points obtained from a scan */
147  //void limit(mrpt::slam::CSimplePointsMap &theMap);
148 
149  /** Set high between ptu tilt axis and hokuyo laser scan */
150 
151  void setHigh(const double &newHigh) { high = newHigh; }
152 
153  /** Obtain a observation from the laser */
154 
155  bool obtainObs( mrpt::slam::CObservation2DRangeScan & obs );
156 
157  /** This method can or cannot be implemented in the derived class, depending on the need for it.
158  * \exception This method must throw an exception with a descriptive message if some critical error is found.
159  */
160  void initialize();
161 
162  /** This method will be invoked at a minimum rate of "process_rate" (Hz)
163  * \exception This method must throw an exception with a descriptive message if some critical error is found.
164  */
165  void doProcess();
166 
167 
168  private:
169 
170  /** Save a observation from the laser into a vector of
171  * observations, calculating sensor position
172  */
173 
174  double saveObservation(const char &axis, const int &mean);
175 
176  /** Performs a simple scan
177  * \param <axis> Pan or Till
178  * \param <tWait> Wait time betwen commands
179  * \param <movements> number total of movements
180  * \param <radPre> radians precision for the scan
181  * \param <vObs> reference to obsevations vector for save the observation
182  */
183 
184  bool singleScan(const char &axis, const int &tWait, const int &movements, const double &radPre, const int &mean);
185 
186  /** Calculate minimum lenght of scan vectors */
187 
188  int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs, std::vector<mrpt::slam::CObservation2DRangeScan> &vObsAux);
189 
190  /** Calculate minimum lenght of 2 scan vectors */
191 
192  int minLengthVectors(mrpt::slam::CObservation2DRangeScan &obs1, mrpt::slam::CObservation2DRangeScan &obs2, const int &mode);
193 
194  /** Load observations in a points map */
195  //void loadObs2PointsMap(mrpt::slam::CSimplePointsMap &theMap);
196 
197  /** Limit the valid position of scan points */
198  //bool limitScan(const char &axis, double &low, double &high, mrpt::slam::CSimplePointsMap &theMap);
199 
200  /** Refine the observations obtains from a continuous scan */
201  void refineVObs(const char &axis);
202 
203  /** Calculate the sensor pose depending teh axis of movements and the ptu position */
204 
205  void calculateSensorPose(const char &axis, const double &pos, mrpt::slam::CObservation2DRangeScan &obs);
206 
207  /** Obtain position of observations between first and second position in m_my_pos map */
208 
209  int obsPosition();
210 
211 
212  }; // End of class
213 
214  } // End of namespace
215 
216 } // End of namespace
217 
218 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013