MRPT  1.9.9
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-2018, 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::obs;
19 using namespace mrpt::io;
20 using namespace mrpt::hwdrivers;
21 
22 /*-------------------------------------------------------------
23  Constructor
24 -------------------------------------------------------------*/
25 C2DRangeFinderAbstract::C2DRangeFinderAbstract()
26  : mrpt::system::COutputLogger("C2DRangeFinderAbstract"),
27  m_lastObservation(),
28  m_lastObservationIsNew(false),
29  m_hardwareError(false),
30  m_nextObservation(),
31  m_showPreview(false),
32  m_stream(nullptr)
33 {
34 }
35 
36 /*-------------------------------------------------------------
37  Destructor
38 -------------------------------------------------------------*/
40 /*-------------------------------------------------------------
41  bindIO
42 -------------------------------------------------------------*/
44 {
45  m_csChangeStream.lock();
46  m_stream = streamIO;
47  m_csChangeStream.unlock();
48 }
49 
50 /*-------------------------------------------------------------
51  getObservation
52 -------------------------------------------------------------*/
54  bool& outThereIsObservation,
55  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
56 {
57  m_csLastObservation.lock();
58 
59  hardwareError = m_hardwareError;
60  outThereIsObservation = m_lastObservationIsNew;
61 
62  if (outThereIsObservation) outObservation = m_lastObservation;
63 
64  m_csLastObservation.unlock();
65 }
66 
67 /*-------------------------------------------------------------
68  doProcess
69 -------------------------------------------------------------*/
71 {
72  bool thereIs, hwError;
73 
74  if (!m_nextObservation)
76  mrpt::make_aligned_shared<CObservation2DRangeScan>();
77 
78  doProcessSimple(thereIs, *m_nextObservation, hwError);
79 
80  if (hwError)
81  {
82  m_state = ssError;
83  THROW_EXCEPTION("Couldn't communicate to the USB board!");
84  }
85 
86  if (thereIs)
87  {
89 
91  m_nextObservation.reset(); // Create a new object in the next call
92  }
93 }
94 
95 /*-------------------------------------------------------------
96  loadExclusionAreas
97 -------------------------------------------------------------*/
99  const mrpt::config::CConfigFileBase& configSource,
100  const std::string& iniSection)
101 {
102  // Params:
103  m_showPreview = configSource.read_bool(iniSection, "preview", false);
104 
105  // Load exclusion areas:
106  m_lstExclusionPolys.clear();
107  m_lstExclusionAngles.clear();
108 
109  unsigned int N = 1;
110 
111  for (;;)
112  {
113  vector<double> x, y, z_range;
114  configSource.read_vector(
115  iniSection, format("exclusionZone%u_x", N), vector<double>(0), x);
116  configSource.read_vector(
117  iniSection, format("exclusionZone%u_y", N), vector<double>(0), y);
118  configSource.read_vector(
119  iniSection, format("exclusionZone%u_z", N++), vector<double>(0),
120  z_range);
121 
122  if (!x.empty() && !y.empty())
123  {
124  ASSERT_(x.size() == y.size());
125  CObservation2DRangeScan::TListExclusionAreasWithRanges::value_type
126  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_(
137  z_range.size() == 2,
138  "exclusionZone%u_z must be a range [z_min z_max]");
139  ASSERT_(z_range[0] <= z_range[1]);
140 
141  dat.second.first = z_range[0];
142  dat.second.second = z_range[1];
143  }
144 
145  m_lstExclusionPolys.push_back(dat);
146  }
147  else
148  break;
149  }
150 
151  // Load forbiden angles;
152  N = 1;
153 
154  for (;;)
155  {
156  const double ini = DEG2RAD(
157  configSource.read_double(
158  iniSection, format("exclusionAngles%u_ini", N), -1000));
159  const double end = DEG2RAD(
160  configSource.read_double(
161  iniSection, format("exclusionAngles%u_end", N++), -1000));
162 
163  if (ini > -M_PI && end > -M_PI)
164  m_lstExclusionAngles.push_back(make_pair(ini, end));
165  else
166  break;
167  }
168 }
169 
170 /*-------------------------------------------------------------
171  filterByExclusionAreas
172 -------------------------------------------------------------*/
175 {
177 }
178 
179 /*-------------------------------------------------------------
180  filterByExclusionAngles
181 -------------------------------------------------------------*/
184 {
186 }
187 
190 {
191  using namespace mrpt::opengl;
192 
193  // show laser scan
194  if (m_showPreview)
195  {
196  if (!m_win)
197  {
198  string caption = string("Preview of ") + m_sensorLabel;
199  m_win = mrpt::make_aligned_shared<mrpt::gui::CDisplayWindow3D>(
200  caption, 640, 480);
201  m_win->setCameraAzimuthDeg(180);
202  m_win->setCameraElevationDeg(90);
203  COpenGLScene::Ptr& theScene = m_win->get3DSceneAndLock();
204  theScene->insert(
205  CAxis::Ptr(
206  mrpt::make_aligned_shared<CAxis>(
207  -300, -300, -50, 300, 300, 50, 1.0, 3, true)));
208  m_win->unlockAccess3DScene();
209  }
210 
211  if (m_win && m_win->isOpen())
212  {
213  COpenGLScene::Ptr& theScene = m_win->get3DSceneAndLock();
215  CRenderizable::Ptr obj = theScene->getByName("laser");
216  if (!obj)
217  {
218  laser = mrpt::make_aligned_shared<opengl::CPlanarLaserScan>();
219  laser->setName("laser");
220  laser->setScan(obs);
221  theScene->insert(laser);
222  }
223  else
224  {
225  laser = std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
226  laser->setScan(obs);
227  }
228  m_win->unlockAccess3DScene();
229  m_win->forceRepaint();
230  } // end if
231  } // end if
232 }
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...
mrpt::obs::CObservation2DRangeScan m_lastObservation
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...
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:41
mrpt::gui::CDisplayWindow3D::Ptr m_win
std::string m_sensorLabel
See CGenericSensor.
double DEG2RAD(const double x)
Degrees to radians.
std::mutex m_csChangeStream
For being thread-safe.
Contains classes for various device interfaces.
STL namespace.
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
mrpt::system::COutputLogger COutputLogger
GLsizei GLsizei GLuint * obj
Definition: glext.h:4070
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
mrpt::io::CStream * m_stream
The I/O channel (will be nullptr if not bound).
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: io/CStream.h:28
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 ...
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This class allows loading and storing values and vectors of different types from a configuration text...
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 ...
GLuint GLuint end
Definition: glext.h:3528
This namespace contains representation of robot actions and observations.
This object renders a 2D laser scan by means of three elements: the points, the line along end-points...
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:101
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
GLsizei const GLchar ** string
Definition: glext.h:4101
mrpt::obs::CObservation2DRangeScan::TListExclusionAreasWithRanges m_lstExclusionPolys
A list of optional exclusion polygons, in coordinates relative to the vehicle, that is...
mrpt::obs::CObservation2DRangeScan::Ptr m_nextObservation
A dynamic object used as buffer in doProcess.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
bool m_showPreview
If true, shows a 3D window with a preview of the grabber data.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
void loadCommonParams(const mrpt::config::CConfigFileBase &configSource, const std::string &iniSection)
Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles)...
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.
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
GLenum GLint GLint y
Definition: glext.h:3538
GLenum GLsizei GLenum format
Definition: glext.h:3531
GLenum GLint x
Definition: glext.h:3538
void filterByExclusionAreas(const TListExclusionAreas &areas)
Mark as invalid sensed points that fall within any of a set of "exclusion areas", given in coordinate...
void bindIO(mrpt::io::CStream *streamIO)
Binds the object to a given I/O channel.
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 ...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 7d5e6d718 Fri Aug 24 01:51:28 2018 +0200 at lun nov 2 08:35:50 CET 2020