MRPT  1.9.9
C2DRangeFinderAbstract.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://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 {
28 }
29 
30 /*-------------------------------------------------------------
31  Destructor
32 -------------------------------------------------------------*/
34 /*-------------------------------------------------------------
35  bindIO
36 -------------------------------------------------------------*/
38 {
39  m_csChangeStream.lock();
40  m_stream = streamIO;
41  m_csChangeStream.unlock();
42 }
43 
44 /*-------------------------------------------------------------
45  getObservation
46 -------------------------------------------------------------*/
48  bool& outThereIsObservation,
49  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
50 {
51  m_csLastObservation.lock();
52 
53  hardwareError = m_hardwareError;
54  outThereIsObservation = m_lastObservationIsNew;
55 
56  if (outThereIsObservation) outObservation = m_lastObservation;
57 
58  m_csLastObservation.unlock();
59 }
60 
61 /*-------------------------------------------------------------
62  doProcess
63 -------------------------------------------------------------*/
65 {
66  bool thereIs, hwError;
67 
68  if (!m_nextObservation)
69  m_nextObservation = std::make_shared<CObservation2DRangeScan>();
70 
71  doProcessSimple(thereIs, *m_nextObservation, hwError);
72 
73  if (hwError)
74  {
75  m_state = ssError;
77  5.0, "Error reading from the sensor hardware. Will retry.");
78  }
79 
80  if (thereIs)
81  {
83 
85  m_nextObservation.reset(); // Create a new object in the next call
86  }
87 }
88 
90 {
91  const auto new_t = mrpt::system::now();
92 
94  {
98  }
99  m_last_good_scan = new_t;
101 }
102 
103 // Returns true if ok, false if this seems to be an error
105 {
106  if (m_last_good_scan == INVALID_TIMESTAMP) return true;
107 
108  const double dt =
110 
111  if (dt > 1.50 * m_estimated_scan_period)
113  return false;
114 
115  return true;
116 }
117 
118 /*-------------------------------------------------------------
119  loadExclusionAreas
120 -------------------------------------------------------------*/
122  const mrpt::config::CConfigFileBase& configSource,
123  const std::string& iniSection)
124 {
125  // Params:
126  m_showPreview = configSource.read_bool(iniSection, "preview", false);
127 
128  // Load exclusion areas:
129  m_lstExclusionPolys.clear();
130  m_lstExclusionAngles.clear();
131 
132  unsigned int N = 1;
133 
134  for (;;)
135  {
136  vector<double> x, y, z_range;
137  configSource.read_vector(
138  iniSection, format("exclusionZone%u_x", N), vector<double>(0), x);
139  configSource.read_vector(
140  iniSection, format("exclusionZone%u_y", N), vector<double>(0), y);
141  configSource.read_vector(
142  iniSection, format("exclusionZone%u_z", N++), vector<double>(0),
143  z_range);
144 
145  if (!x.empty() && !y.empty())
146  {
147  ASSERT_(x.size() == y.size());
148  CObservation2DRangeScan::TListExclusionAreasWithRanges::value_type
149  dat;
150 
151  dat.first.setAllVertices(x, y);
152  if (z_range.empty())
153  {
154  dat.second.first = -std::numeric_limits<double>::max();
155  dat.second.second = std::numeric_limits<double>::max();
156  }
157  else
158  {
159  ASSERTMSG_(
160  z_range.size() == 2,
161  "exclusionZone%u_z must be a range [z_min z_max]");
162  ASSERT_(z_range[0] <= z_range[1]);
163 
164  dat.second.first = z_range[0];
165  dat.second.second = z_range[1];
166  }
167 
168  m_lstExclusionPolys.push_back(dat);
169  }
170  else
171  break;
172  }
173 
174  // Load forbiden angles;
175  N = 1;
176 
177  for (;;)
178  {
179  const double ini = DEG2RAD(configSource.read_double(
180  iniSection, format("exclusionAngles%u_ini", N), -1000));
181  const double end = DEG2RAD(configSource.read_double(
182  iniSection, format("exclusionAngles%u_end", N++), -1000));
183 
184  if (ini > -M_PI && end > -M_PI)
185  m_lstExclusionAngles.emplace_back(ini, end);
186  else
187  break;
188  }
189 
190  // Max. missed scan failures:
191  m_max_missed_scan_failures = configSource.read_int(
192  iniSection, "maxMissedScansToDeclareError", m_max_missed_scan_failures);
193 }
194 
195 /*-------------------------------------------------------------
196  filterByExclusionAreas
197 -------------------------------------------------------------*/
200 {
202 }
203 
204 /*-------------------------------------------------------------
205  filterByExclusionAngles
206 -------------------------------------------------------------*/
209 {
211 }
212 
215 {
216  using namespace mrpt::opengl;
217 
218  // show laser scan
219  if (m_showPreview)
220  {
221  if (!m_win)
222  {
223  string caption = string("Preview of ") + m_sensorLabel;
224  m_win = mrpt::gui::CDisplayWindow3D::Create(caption, 640, 480);
225  m_win->setCameraAzimuthDeg(180);
226  m_win->setCameraElevationDeg(90);
227  COpenGLScene::Ptr& theScene = m_win->get3DSceneAndLock();
228  theScene->insert(CAxis::Ptr(std::make_shared<CAxis>(
229  -300, -300, -50, 300, 300, 50, 1.0, 3, true)));
230  m_win->unlockAccess3DScene();
231  }
232 
233  if (m_win && m_win->isOpen())
234  {
235  COpenGLScene::Ptr& theScene = m_win->get3DSceneAndLock();
237  CRenderizable::Ptr obj = theScene->getByName("laser");
238  if (!obj)
239  {
240  laser = std::make_shared<opengl::CPlanarLaserScan>();
241  laser->setName("laser");
242  laser->setScan(obs);
243  theScene->insert(laser);
244  }
245  else
246  {
247  laser = std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
248  laser->setScan(obs);
249  }
250  m_win->unlockAccess3DScene();
251  m_win->forceRepaint();
252  } // end if
253  } // end if
254 }
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.
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::gui::CDisplayWindow3D::Ptr m_win
void internal_notifyGoodScanNow()
Must be called from doProcessSimple() implementations.
std::string m_sensorLabel
See CGenericSensor.
std::mutex m_csChangeStream
For being thread-safe.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
Contains classes for various device interfaces.
mrpt::system::TTimeStamp m_last_good_scan
Used in internal_notifyGoodScanNow()
STL namespace.
static CDisplayWindow3D::Ptr Create(const std::string &windowCaption, unsigned int initialWindowWidth=400, unsigned int initialWindowHeight=300)
Class factory returning a smart pointer.
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:4085
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
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
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:120
double m_estimated_scan_period
Updated in internal_notifyGoodScanNow()
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 ...
constexpr double DEG2RAD(const double x)
Degrees to radians.
GLuint GLuint end
Definition: glext.h:3532
void doProcess() override
Main method for a CGenericSensor.
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:108
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
GLsizei const GLchar ** string
Definition: glext.h:4116
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.
#define MRPT_LOG_THROTTLE_ERROR(_PERIOD_SECONDS, _STRING)
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 internal_notifyNoScanReceived()
Must be called from doProcessSimple() implementations.
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.
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)...
int m_failure_waiting_scan_counter
Used in internal_notifyNoScanReceived()
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:3542
GLenum GLsizei GLenum format
Definition: glext.h:3535
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.h:123
~C2DRangeFinderAbstract() override
Destructor.
GLenum GLint x
Definition: glext.h:3542
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.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
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: cb560b230 Wed Nov 13 08:06:48 2019 +0100 at miƩ nov 13 08:15:10 CET 2019