MRPT  2.0.1
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-2020, 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 
36 {
37  m_csChangeStream.lock();
38  m_stream = streamIO;
39  m_csChangeStream.unlock();
40 }
41 
42 /*-------------------------------------------------------------
43  getObservation
44 -------------------------------------------------------------*/
46  bool& outThereIsObservation,
47  mrpt::obs::CObservation2DRangeScan& outObservation, bool& hardwareError)
48 {
49  m_csLastObservation.lock();
50 
51  hardwareError = m_hardwareError;
52  outThereIsObservation = m_lastObservationIsNew;
53 
54  if (outThereIsObservation) outObservation = m_lastObservation;
55 
56  m_csLastObservation.unlock();
57 }
58 
59 /*-------------------------------------------------------------
60  doProcess
61 -------------------------------------------------------------*/
63 {
64  bool thereIs, hwError;
65 
66  if (!m_nextObservation)
67  m_nextObservation = std::make_shared<CObservation2DRangeScan>();
68 
69  doProcessSimple(thereIs, *m_nextObservation, hwError);
70 
71  if (hwError)
72  {
73  m_state = ssError;
75  5.0, "Error reading from the sensor hardware. Will retry.");
76  }
77 
78  if (thereIs)
79  {
81 
83  m_nextObservation.reset(); // Create a new object in the next call
84  }
85 }
86 
88 {
89  const auto new_t = mrpt::system::now();
90 
92  {
96  }
97  m_last_good_scan = new_t;
99 }
100 
101 // Returns true if ok, false if this seems to be an error
103 {
104  if (m_last_good_scan == INVALID_TIMESTAMP) return true;
105 
106  const double dt =
108 
109  if (dt > 1.50 * m_estimated_scan_period)
111  return false;
112 
113  return true;
114 }
115 
116 /*-------------------------------------------------------------
117  loadExclusionAreas
118 -------------------------------------------------------------*/
120  const mrpt::config::CConfigFileBase& configSource,
121  const std::string& iniSection)
122 {
123  // Params:
124  m_showPreview = configSource.read_bool(iniSection, "preview", false);
125 
126  // Load exclusion areas:
127  m_lstExclusionPolys.clear();
128  m_lstExclusionAngles.clear();
129 
130  unsigned int N = 1;
131 
132  for (;;)
133  {
134  vector<double> x, y, z_range;
135  configSource.read_vector(
136  iniSection, format("exclusionZone%u_x", N), vector<double>(0), x);
137  configSource.read_vector(
138  iniSection, format("exclusionZone%u_y", N), vector<double>(0), y);
139  configSource.read_vector(
140  iniSection, format("exclusionZone%u_z", N++), vector<double>(0),
141  z_range);
142 
143  if (!x.empty() && !y.empty())
144  {
145  ASSERT_(x.size() == y.size());
146  CObservation2DRangeScan::TListExclusionAreasWithRanges::value_type
147  dat;
148 
149  dat.first.setAllVertices(x, y);
150  if (z_range.empty())
151  {
152  dat.second.first = -std::numeric_limits<double>::max();
153  dat.second.second = std::numeric_limits<double>::max();
154  }
155  else
156  {
157  ASSERTMSG_(
158  z_range.size() == 2,
159  "exclusionZone%u_z must be a range [z_min z_max]");
160  ASSERT_(z_range[0] <= z_range[1]);
161 
162  dat.second.first = z_range[0];
163  dat.second.second = z_range[1];
164  }
165 
166  m_lstExclusionPolys.push_back(dat);
167  }
168  else
169  break;
170  }
171 
172  // Load forbiden angles;
173  N = 1;
174 
175  for (;;)
176  {
177  const double ini = DEG2RAD(configSource.read_double(
178  iniSection, format("exclusionAngles%u_ini", N), -1000));
179  const double end = DEG2RAD(configSource.read_double(
180  iniSection, format("exclusionAngles%u_end", N++), -1000));
181 
182  if (ini > -M_PI && end > -M_PI)
183  m_lstExclusionAngles.emplace_back(ini, end);
184  else
185  break;
186  }
187 
188  // Max. missed scan failures:
189  m_max_missed_scan_failures = configSource.read_int(
190  iniSection, "maxMissedScansToDeclareError", m_max_missed_scan_failures);
191 }
192 
193 /*-------------------------------------------------------------
194  filterByExclusionAreas
195 -------------------------------------------------------------*/
198 {
200 }
201 
202 /*-------------------------------------------------------------
203  filterByExclusionAngles
204 -------------------------------------------------------------*/
207 {
209 }
210 
213 {
214  using namespace mrpt::opengl;
215 
216  // show laser scan
217  if (m_showPreview)
218  {
219  if (!m_win)
220  {
221  string caption = string("Preview of ") + m_sensorLabel;
222  m_win = mrpt::gui::CDisplayWindow3D::Create(caption, 640, 480);
223  m_win->setCameraAzimuthDeg(180);
224  m_win->setCameraElevationDeg(90);
225  COpenGLScene::Ptr& theScene = m_win->get3DSceneAndLock();
226  theScene->insert(std::make_shared<CAxis>(
227  -300, -300, -50, 300, 300, 50, 1.0, 3, true));
228  m_win->unlockAccess3DScene();
229  }
230 
231  if (m_win && m_win->isOpen())
232  {
233  COpenGLScene::Ptr& theScene = m_win->get3DSceneAndLock();
235  CRenderizable::Ptr obj = theScene->getByName("laser");
236  if (!obj)
237  {
238  laser = std::make_shared<opengl::CPlanarLaserScan>();
239  laser->setName("laser");
240  laser->setScan(obs);
241  theScene->insert(laser);
242  }
243  else
244  {
245  laser = std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
246  laser->setScan(obs);
247  }
248  m_win->unlockAccess3DScene();
249  m_win->forceRepaint();
250  } // end if
251  } // end if
252 }
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
void bindIO(const std::shared_ptr< mrpt::io::CStream > &streamIO)
Binds the object to a given I/O channel.
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
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
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.
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
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)
const_iterator end() const
Definition: ts_hash_map.h:246
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...
std::shared_ptr< mrpt::io::CStream > m_stream
The I/O channel (will be nullptr if not bound).
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:13
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.
void filterByExclusionAreas(const TListExclusionAreas &areas)
Mark as invalid sensed points that fall within any of a set of "exclusion areas", given in coordinate...
#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 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020