MRPT  2.0.3
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 
35 void C2DRangeFinderAbstract::bindIO(const std::shared_ptr<CStream>& streamIO)
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 }
os.h
mrpt::system::timeDifference
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
mrpt::hwdrivers::C2DRangeFinderAbstract::m_win
mrpt::gui::CDisplayWindow3D::Ptr m_win
Definition: C2DRangeFinderAbstract.h:65
mrpt::containers::end
const_iterator end() const
Definition: ts_hash_map.h:246
INVALID_TIMESTAMP
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
mrpt::io
Definition: img/CImage.h:24
mrpt::gui::CDisplayWindow3D::Create
static CDisplayWindow3D::Ptr Create(const std::string &windowCaption, unsigned int initialWindowWidth=400, unsigned int initialWindowHeight=300)
Class factory returning a smart pointer.
Definition: CDisplayWindow3D.cpp:387
mrpt::hwdrivers::C2DRangeFinderAbstract::internal_notifyNoScanReceived
bool internal_notifyNoScanReceived()
Must be called from doProcessSimple() implementations.
Definition: C2DRangeFinderAbstract.cpp:102
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::hwdrivers::C2DRangeFinderAbstract::bindIO
void bindIO(const std::shared_ptr< mrpt::io::CStream > &streamIO)
Binds the object to a given I/O channel.
Definition: C2DRangeFinderAbstract.cpp:35
mrpt::config::CConfigFileBase::read_double
double read_double(const std::string &section, const std::string &name, double defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:106
mrpt::hwdrivers::C2DRangeFinderAbstract::~C2DRangeFinderAbstract
~C2DRangeFinderAbstract() override
Destructor.
mrpt::hwdrivers::C2DRangeFinderAbstract::doProcessSimple
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::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAngles
void filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those ranges in a set of forbiden angle ranges.
Definition: C2DRangeFinderAbstract.cpp:205
mrpt::system::now
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
mrpt::hwdrivers::CGenericSensor::ssWorking
@ ssWorking
Definition: CGenericSensor.h:87
mrpt::config::CConfigFileBase::read_bool
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:155
mrpt::hwdrivers
Contains classes for various device interfaces.
Definition: C2DRangeFinderAbstract.h:19
mrpt::hwdrivers::CGenericSensor::ssError
@ ssError
Definition: CGenericSensor.h:88
mrpt::opengl::CRenderizable::Ptr
std::shared_ptr< CRenderizable > Ptr
Definition: CRenderizable.h:50
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::hwdrivers::C2DRangeFinderAbstract::internal_notifyGoodScanNow
void internal_notifyGoodScanNow()
Must be called from doProcessSimple() implementations.
Definition: C2DRangeFinderAbstract.cpp:87
mrpt::utils::COutputLogger
mrpt::system::COutputLogger COutputLogger
Definition: utils/COutputLogger.h:5
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::hwdrivers::CGenericSensor::m_state
TSensorState m_state
Definition: CGenericSensor.h:148
mrpt::hwdrivers::C2DRangeFinderAbstract::m_hardwareError
bool m_hardwareError
Definition: C2DRangeFinderAbstract.h:46
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::hwdrivers::C2DRangeFinderAbstract::m_failure_waiting_scan_counter
int m_failure_waiting_scan_counter
Used in internal_notifyNoScanReceived()
Definition: C2DRangeFinderAbstract.h:180
mrpt::hwdrivers::C2DRangeFinderAbstract::m_last_good_scan
mrpt::system::TTimeStamp m_last_good_scan
Used in internal_notifyGoodScanNow()
Definition: C2DRangeFinderAbstract.h:176
mrpt::hwdrivers::C2DRangeFinderAbstract::getObservation
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 ...
Definition: C2DRangeFinderAbstract.cpp:45
mrpt::config::CConfigFileBase::read_int
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Definition: CConfigFileBase.cpp:130
mrpt::obs::CObservation2DRangeScan::filterByExclusionAngles
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...
Definition: CObservation2DRangeScan.cpp:344
mrpt::hwdrivers::C2DRangeFinderAbstract::m_lstExclusionPolys
mrpt::obs::CObservation2DRangeScan::TListExclusionAreasWithRanges m_lstExclusionPolys
A list of optional exclusion polygons, in coordinates relative to the vehicle, that is,...
Definition: C2DRangeFinderAbstract.h:58
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::hwdrivers::C2DRangeFinderAbstract::loadCommonParams
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).
Definition: C2DRangeFinderAbstract.cpp:119
mrpt::hwdrivers::C2DRangeFinderAbstract::m_nextObservation
mrpt::obs::CObservation2DRangeScan::Ptr m_nextObservation
A dynamic object used as buffer in doProcess.
Definition: C2DRangeFinderAbstract.h:53
mrpt::hwdrivers::C2DRangeFinderAbstract::m_csChangeStream
std::mutex m_csChangeStream
For being thread-safe.
Definition: C2DRangeFinderAbstract.h:50
mrpt::hwdrivers::C2DRangeFinderAbstract::m_lastObservationIsNew
bool m_lastObservationIsNew
Definition: C2DRangeFinderAbstract.h:45
mrpt::hwdrivers::C2DRangeFinderAbstract::m_max_missed_scan_failures
int m_max_missed_scan_failures
Definition: C2DRangeFinderAbstract.h:180
mrpt::hwdrivers::CGenericSensor::appendObservation
void appendObservation(const mrpt::serialization::CSerializable::Ptr &obj)
Like appendObservations() but for just one observation.
Definition: CGenericSensor.h:180
mrpt::hwdrivers::C2DRangeFinderAbstract::m_estimated_scan_period
double m_estimated_scan_period
Updated in internal_notifyGoodScanNow()
Definition: C2DRangeFinderAbstract.h:178
mrpt::opengl::CPlanarLaserScan::Ptr
std::shared_ptr< mrpt::opengl ::CPlanarLaserScan > Ptr
Definition: CPlanarLaserScan.h:61
mrpt::hwdrivers::CGenericSensor::m_sensorLabel
std::string m_sensorLabel
See CGenericSensor.
Definition: CGenericSensor.h:141
mrpt::DEG2RAD
constexpr double DEG2RAD(const double x)
Degrees to radians
Definition: core/include/mrpt/core/bits_math.h:47
mrpt::hwdrivers::C2DRangeFinderAbstract::m_showPreview
bool m_showPreview
If true, shows a 3D window with a preview of the grabber data.
Definition: C2DRangeFinderAbstract.h:64
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::hwdrivers::C2DRangeFinderAbstract::processPreview
void processPreview(const mrpt::obs::CObservation2DRangeScan &obs)
Must be called inside the capture method to allow optional GUI preview of scans.
Definition: C2DRangeFinderAbstract.cpp:211
mrpt::opengl::COpenGLScene::Ptr
std::shared_ptr< mrpt::opengl ::COpenGLScene > Ptr
Definition: COpenGLScene.h:58
mrpt::hwdrivers::C2DRangeFinderAbstract::m_lastObservation
mrpt::obs::CObservation2DRangeScan m_lastObservation
Definition: C2DRangeFinderAbstract.h:44
mrpt::hwdrivers::C2DRangeFinderAbstract::m_lstExclusionAngles
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 ...
Definition: C2DRangeFinderAbstract.h:61
CAxis.h
mrpt::hwdrivers::C2DRangeFinderAbstract::m_stream
std::shared_ptr< mrpt::io::CStream > m_stream
The I/O channel (will be nullptr if not bound).
Definition: C2DRangeFinderAbstract.h:69
M_PI
#define M_PI
Definition: core/include/mrpt/core/bits_math.h:43
mrpt::hwdrivers::C2DRangeFinderAbstract::m_csLastObservation
std::mutex m_csLastObservation
Definition: C2DRangeFinderAbstract.h:50
MRPT_LOG_THROTTLE_ERROR
#define MRPT_LOG_THROTTLE_ERROR(_PERIOD_SECONDS, _STRING)
Definition: system/COutputLogger.h:456
CPlanarLaserScan.h
mrpt::obs::CObservation2DRangeScan::filterByExclusionAreas
void filterByExclusionAreas(const TListExclusionAreas &areas)
Mark as invalid sensed points that fall within any of a set of "exclusion areas", given in coordinate...
Definition: CObservation2DRangeScan.cpp:323
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::hwdrivers::C2DRangeFinderAbstract::doProcess
void doProcess() override
Main method for a CGenericSensor.
Definition: C2DRangeFinderAbstract.cpp:62
mrpt::config::CConfigFileBase::read_vector
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 ....
Definition: config/CConfigFileBase.h:194
mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAreas
void filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan &obs) const
Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.
Definition: C2DRangeFinderAbstract.cpp:196
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
hwdrivers-precomp.h
C2DRangeFinderAbstract.h



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020