Main MRPT website > C++ reference for MRPT 1.5.6
C2DRangeFinderAbstract.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "hwdrivers-precomp.h" // Precompiled headers
11 
13 #include <mrpt/opengl/CAxis.h>
14 #include <mrpt/opengl/CPlanarLaserScan.h> // in library mrpt-maps
15 #include <mrpt/system/os.h>
16 
17 using namespace std;
18 using namespace mrpt::utils;
19 using namespace mrpt::obs;
20 using namespace mrpt::hwdrivers;
21 
22 /*-------------------------------------------------------------
23  Constructor
24 -------------------------------------------------------------*/
25 C2DRangeFinderAbstract::C2DRangeFinderAbstract() :
26  mrpt::utils::COutputLogger("C2DRangeFinderAbstract"),
27  m_lastObservation ( ),
28  m_lastObservationIsNew ( false ),
29  m_hardwareError ( false ),
30  m_nextObservation (),
31  m_showPreview ( false ),
32  m_stream ( NULL )
33 {
34 }
35 
36 /*-------------------------------------------------------------
37  Destructor
38 -------------------------------------------------------------*/
40 {
41 }
42 
43 /*-------------------------------------------------------------
44  bindIO
45 -------------------------------------------------------------*/
47 {
49  m_stream = streamIO;
51 }
52 
53 /*-------------------------------------------------------------
54  getObservation
55 -------------------------------------------------------------*/
57  bool &outThereIsObservation,
58  mrpt::obs::CObservation2DRangeScan &outObservation,
59  bool &hardwareError )
60 {
62 
63  hardwareError = m_hardwareError;
64  outThereIsObservation = m_lastObservationIsNew;
65 
66  if (outThereIsObservation)
67  outObservation = m_lastObservation;
68 
70 }
71 
72 /*-------------------------------------------------------------
73  doProcess
74 -------------------------------------------------------------*/
76 {
77  bool thereIs, hwError;
78 
79  if (!m_nextObservation)
80  m_nextObservation = CObservation2DRangeScan::Create();
81 
82  doProcessSimple( thereIs, *m_nextObservation, hwError );
83 
84  if (hwError)
85  {
86  m_state = ssError;
87  THROW_EXCEPTION("Couldn't communicate to the USB board!");
88  }
89 
90  if (thereIs)
91  {
93 
95  m_nextObservation.clear_unique(); // Create a new object in the next call
96  }
97 }
98 
99 /*-------------------------------------------------------------
100  loadExclusionAreas
101 -------------------------------------------------------------*/
103  const mrpt::utils::CConfigFileBase &configSource,
104  const std::string &iniSection )
105 {
106  // Params:
107  m_showPreview = configSource.read_bool(iniSection, "preview", false );
108 
109  // Load exclusion areas:
110  m_lstExclusionPolys.clear();
111  m_lstExclusionAngles.clear();
112 
113  unsigned int N = 1;
114 
115  for(;;)
116  {
117  vector<double> x,y, z_range;
118  configSource.read_vector( iniSection, format("exclusionZone%u_x",N), vector<double>(0), x);
119  configSource.read_vector( iniSection, format("exclusionZone%u_y",N), vector<double>(0), y);
120  configSource.read_vector( iniSection, format("exclusionZone%u_z",N++), vector<double>(0), z_range);
121 
122  if (!x.empty() && !y.empty())
123  {
124  ASSERT_(x.size()==y.size())
125 
126  CObservation2DRangeScan::TListExclusionAreasWithRanges::value_type dat;
127 
128  dat.first.setAllVertices(x,y);
129  if (z_range.empty())
130  {
131  dat.second.first = -std::numeric_limits<double>::max();
132  dat.second.second = std::numeric_limits<double>::max();
133  }
134  else
135  {
136  ASSERTMSG_(z_range.size()==2,"exclusionZone%u_z must be a range [z_min z_max]");
137  ASSERT_(z_range[0]<=z_range[1]);
138 
139  dat.second.first = z_range[0];
140  dat.second.second = z_range[1];
141  }
142 
143  m_lstExclusionPolys.push_back(dat);
144  }
145  else break;
146  }
147 
148 
149  // Load forbiden angles;
150  N = 1;
151 
152  for(;;)
153  {
154  const double ini = DEG2RAD( configSource.read_double( iniSection, format("exclusionAngles%u_ini",N), -1000 ) );
155  const double end = DEG2RAD( configSource.read_double( iniSection, format("exclusionAngles%u_end",N++), -1000 ) );
156 
157  if (ini>-M_PI && end>-M_PI)
158  m_lstExclusionAngles.push_back(make_pair(ini,end));
159  else break;
160  }
161 }
162 
163 /*-------------------------------------------------------------
164  filterByExclusionAreas
165 -------------------------------------------------------------*/
167 {
169 }
170 
171 /*-------------------------------------------------------------
172  filterByExclusionAngles
173 -------------------------------------------------------------*/
175 {
177 }
178 
179 
181 {
182  using namespace mrpt::opengl;
183 
184  // show laser scan
185  if( m_showPreview )
186  {
187  if( !m_win )
188  {
189  string caption = string("Preview of ")+m_sensorLabel;
190  m_win = mrpt::gui::CDisplayWindow3D::Create( caption, 640, 480 );
191  m_win->setCameraAzimuthDeg(180);
192  m_win->setCameraElevationDeg(90);
193  COpenGLScenePtr &theScene = m_win->get3DSceneAndLock();
194  theScene->insert(CAxisPtr( CAxis::Create(-300,-300,-50, 300,300,50, 1.0, 3, true ) ));
195  m_win->unlockAccess3DScene();
196  }
197 
198  if( m_win && m_win->isOpen() )
199  {
200  COpenGLScenePtr &theScene = m_win->get3DSceneAndLock();
201  opengl::CPlanarLaserScanPtr laser;
202  CRenderizablePtr obj = theScene->getByName("laser");
203  if( !obj )
204  {
206  laser->setName("laser");
207  laser->setScan(obs);
208  theScene->insert(laser);
209  }
210  else
211  {
212  laser = CPlanarLaserScanPtr(obj);
213  laser->setScan(obs);
214  }
215  m_win->unlockAccess3DScene();
216  m_win->forceRepaint();
217  } // end if
218  } // end if
219 }
std::vector< std::pair< double, double > > m_lstExclusionAngles
A list of pairs of angles <init,end> such as all sensor ranges falling in those forbiden angles will ...
virtual void doProcessSimple(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError)=0
Specific laser scanner "software drivers" must process here new data from the I/O stream...
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
mrpt::obs::CObservation2DRangeScan m_lastObservation
utils::CStream * m_stream
The I/O channel (will be NULL if not bound).
std::string m_sensorLabel
See CGenericSensor.
#define THROW_EXCEPTION(msg)
synch::CCriticalSection m_csChangeStream
For being thread-safe.
void read_vector(const std::string &section, const std::string &name, const VECTOR_TYPE &defaultValue, VECTOR_TYPE &outValues, bool failIfNotFound=false) const
Reads a configuration parameter of type vector, stored in the file as a string: "[v1 v2 v3 ...
#define M_PI
Definition: bits.h:78
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
This class allows loading and storing values and vectors of different types from a configuration text...
void getObservation(bool &outThereIsObservation, mrpt::obs::CObservation2DRangeScan &outObservation, bool &hardwareError)
Get the last observation from the sensor, if available, and unmarks it as being "the last one" (thus ...
void leave() const MRPT_OVERRIDE
Leave. the calling thread is not the current owner of the critical section.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
static CDisplayWindow3DPtr Create()
GLuint GLuint end
Definition: glew.h:1167
GLint GLint GLsizei GLsizei GLsizei GLint GLenum format
Definition: glew.h:1168
void enter() const MRPT_OVERRIDE
Enter. the calling thread already possesses this critical section (it would be a dead-lock).
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLhandleARB obj
Definition: glew.h:3276
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
void bindIO(mrpt::utils::CStream *streamIO)
Binds the object to a given I/O channel.
#define DEG2RAD
void loadCommonParams(const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
mrpt::obs::CObservation2DRangeScan::TListExclusionAreasWithRanges m_lstExclusionPolys
A list of optional exclusion polygons, in coordinates relative to the vehicle, that is...
mrpt::obs::CObservation2DRangeScanPtr m_nextObservation
A dynamic object used as buffer in doProcess.
void filterByExclusionAngles(const std::vector< std::pair< double, double > > &angles)
Mark as invalid the ranges in any of a given set of "forbiden angle ranges", given as pairs<min_angle...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
static CPlanarLaserScanPtr Create()
bool m_showPreview
If true, shows a 3D window with a preview of the grabber data.
void doProcess()
Main method for a CGenericSensor.
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
#define ASSERT_(f)
void appendObservation(const mrpt::utils::CSerializablePtr &obj)
Like appendObservations() but for just one observation.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
void filterByExclusionAreas(const TListExclusionAreas &areas)
Mark as invalid sensed points that fall within any of a set of "exclusion areas", given in coordinate...
#define ASSERTMSG_(f, __ERROR_MSG)



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018