Debian Codegraph @

libs/hwdrivers/src/C2DRangeFinderAbstract.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, 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 //
12 #include <mrpt/hwdrivers/C2DRangeFinderAbstract.h>
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 -------------------------------------------------------------*/
33 C2DRangeFinderAbstract::~C2DRangeFinderAbstract() = default;
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 -------------------------------------------------------------*/
45 void C2DRangeFinderAbstract::getObservation(
46 bool& outThereIsObservation,
47 mrpt::obs::CObservation2DRangeScan& outObservation,
48 bool& hardwareError)
49 {
50 m_csLastObservation.lock();
51
52 hardwareError = m_hardwareError;
53 outThereIsObservation = m_lastObservationIsNew;
54
55 if (outThereIsObservation) outObservation = m_lastObservation;
56
57 m_csLastObservation.unlock();
58 }
59
60 /*-------------------------------------------------------------
61 doProcess
62 -------------------------------------------------------------*/
63 void C2DRangeFinderAbstract::doProcess()
64 {
65 bool thereIs, hwError;
66
67 if (!m_nextObservation) m_nextObservation = std::make_shared<CObservation2DRangeScan>();
68
69 doProcessSimple(thereIs, *m_nextObservation, hwError);
70
71 if (hwError)
72 {
73 m_state = ssError;
74 MRPT_LOG_THROTTLE_ERROR(5.0, "Error reading from the sensor hardware. Will retry.");
75 }
76
77 if (thereIs)
78 {
79 m_state = ssWorking;
80
81 appendObservation(m_nextObservation);
82 m_nextObservation.reset(); // Create a new object in the next call
83 }
84 }
85
86 void C2DRangeFinderAbstract::internal_notifyGoodScanNow()
87 {
88 const auto new_t = mrpt::Clock::now();
89
90 if (m_last_good_scan != INVALID_TIMESTAMP)
91 {
92 m_estimated_scan_period =
93 0.9 * m_estimated_scan_period + 0.1 * mrpt::system::timeDifference(m_last_good_scan, new_t);
94 }
95 m_last_good_scan = new_t;
96 m_failure_waiting_scan_counter = 0;
97 }
98
99 // Returns true if ok, false if this seems to be an error
100 bool C2DRangeFinderAbstract::internal_notifyNoScanReceived()
101 {
102 if (m_last_good_scan == INVALID_TIMESTAMP) return true;
103
104 const double dt = mrpt::system::timeDifference(m_last_good_scan, mrpt::Clock::now());
105
106 if (dt > 1.50 * m_estimated_scan_period)
107 if (++m_failure_waiting_scan_counter >= m_max_missed_scan_failures) return false;
108
109 return true;
110 }
111
112 /*-------------------------------------------------------------
113 loadExclusionAreas
114 -------------------------------------------------------------*/
115 void C2DRangeFinderAbstract::loadCommonParams(
116 const mrpt::config::CConfigFileBase& configSource, const std::string& iniSection)
117 {
118 // Params:
119 m_showPreview = configSource.read_bool(iniSection, "preview", false);
120
121 // Load exclusion areas:
122 m_lstExclusionPolys.clear();
123 m_lstExclusionAngles.clear();
124
125 unsigned int N = 1;
126
127 for (;;)
128 {
129 vector<double> x, y, z_range;
130 configSource.read_vector(
131 iniSection, mrpt::format("exclusionZone%u_x", N), vector<double>(0), x);
132 configSource.read_vector(
133 iniSection, mrpt::format("exclusionZone%u_y", N), vector<double>(0), y);
134 configSource.read_vector(
135 iniSection, mrpt::format("exclusionZone%u_z", N++), vector<double>(0), z_range);
136
137 if (!x.empty() && !y.empty())
138 {
139 ASSERT_(x.size() == y.size());
140 CObservation2DRangeScan::TListExclusionAreasWithRanges::value_type dat;
141
142 dat.first.setAllVertices(x, y);
143 if (z_range.empty())
144 {
145 dat.second.first = -std::numeric_limits<double>::max();
146 dat.second.second = std::numeric_limits<double>::max();
147 }
148 else
149 {
150 ASSERTMSG_(z_range.size() == 2, "exclusionZone%u_z must be a range [z_min z_max]");
151 ASSERT_(z_range[0] <= z_range[1]);
152
153 dat.second.first = z_range[0];
154 dat.second.second = z_range[1];
155 }
156
157 m_lstExclusionPolys.push_back(dat);
158 }
159 else
160 break;
161 }
162
163 // Load forbiden angles;
164 N = 1;
165
166 for (;;)
167 {
168 const double ini = DEG2RAD(
169 configSource.read_double(iniSection, mrpt::format("exclusionAngles%u_ini", N), -1000));
170 const double end = DEG2RAD(
171 configSource.read_double(iniSection, mrpt::format("exclusionAngles%u_end", N++), -1000));
172
173 if (ini > -M_PI && end > -M_PI)
174 m_lstExclusionAngles.emplace_back(ini, end);
175 else
176 break;
177 }
178
179 // Max. missed scan failures:
180 m_max_missed_scan_failures =
181 configSource.read_int(iniSection, "maxMissedScansToDeclareError", m_max_missed_scan_failures);
182 }
183
184 /*-------------------------------------------------------------
185 filterByExclusionAreas
186 -------------------------------------------------------------*/
187 void C2DRangeFinderAbstract::filterByExclusionAreas(mrpt::obs::CObservation2DRangeScan& obs) const
188 {
189 obs.filterByExclusionAreas(m_lstExclusionPolys);
190 }
191
192 /*-------------------------------------------------------------
193 filterByExclusionAngles
194 -------------------------------------------------------------*/
195 void C2DRangeFinderAbstract::filterByExclusionAngles(mrpt::obs::CObservation2DRangeScan& obs) const
196 {
197 obs.filterByExclusionAngles(m_lstExclusionAngles);
198 }
199
200 void C2DRangeFinderAbstract::processPreview(const mrpt::obs::CObservation2DRangeScan& obs)
201 {
202 using namespace mrpt::opengl;
203
204 // show laser scan
205 if (m_showPreview)
206 {
207 if (!m_win)
208 {
209 string caption = string("Preview of ") + m_sensorLabel;
210 m_win = mrpt::gui::CDisplayWindow3D::Create(caption, 640, 480);
211 m_win->setCameraAzimuthDeg(180);
212 m_win->setCameraElevationDeg(90);
213 Scene::Ptr& theScene = m_win->get3DSceneAndLock();
214 theScene->insert(std::make_shared<CAxis>(-300, -300, -50, 300, 300, 50, 1.0, 3, true));
215 m_win->unlockAccess3DScene();
216 }
217
218 if (m_win && m_win->isOpen())
219 {
220 Scene::Ptr& theScene = m_win->get3DSceneAndLock();
221 opengl::CPlanarLaserScan::Ptr laser;
222 CRenderizable::Ptr obj = theScene->getByName("laser");
223 if (!obj)
224 {
225 laser = std::make_shared<opengl::CPlanarLaserScan>();
226 laser->setName("laser");
227 laser->setScan(obs);
228 theScene->insert(laser);
229 }
230 else
231 {
232 laser = std::dynamic_pointer_cast<CPlanarLaserScan>(obj);
233 laser->setScan(obs);
234 }
235 m_win->unlockAccess3DScene();
236 m_win->forceRepaint();
237 } // end if
238 } // end if
239 }