| 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 |
} |