diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/C2DRangeFinderAbstract.h b/libs/hwdrivers/include/mrpt/hwdrivers/C2DRangeFinderAbstract.h index fcedd8d9c6..8b1cb2acab 100644 --- a/libs/hwdrivers/include/mrpt/hwdrivers/C2DRangeFinderAbstract.h +++ b/libs/hwdrivers/include/mrpt/hwdrivers/C2DRangeFinderAbstract.h @@ -157,5 +157,27 @@ class C2DRangeFinderAbstract : public mrpt::system::COutputLogger, */ virtual bool turnOff() = 0; + /** Returns the empirical, filtered estimation for the period at which whole + * scans are being returned from calls to doProcessSimple() + * \note: Units: seconds */ + double getEstimatedScanPeriod() const { return m_estimated_scan_period; } + + protected: + /** Must be called from doProcessSimple() implementations */ + void internal_notifyGoodScanNow(); + + /** Must be called from doProcessSimple() implementations. + * Returns true if ok, false if this seems strange and should + * return an error condition to the user. */ + bool internal_notifyNoScanReceived(); + + private: + /// Used in internal_notifyGoodScanNow() + mrpt::system::TTimeStamp m_last_good_scan{INVALID_TIMESTAMP}; + /// Updated in internal_notifyGoodScanNow() + double m_estimated_scan_period{1.0}; + /// Used in internal_notifyNoScanReceived() + int m_failure_waiting_scan_counter{0}, m_max_missed_scan_failures{10}; + }; // End of class } // namespace mrpt::hwdrivers diff --git a/libs/hwdrivers/include/mrpt/hwdrivers/CHokuyoURG.h b/libs/hwdrivers/include/mrpt/hwdrivers/CHokuyoURG.h index c9a9a1bd64..7c87bb0c0d 100644 --- a/libs/hwdrivers/include/mrpt/hwdrivers/CHokuyoURG.h +++ b/libs/hwdrivers/include/mrpt/hwdrivers/CHokuyoURG.h @@ -96,7 +96,7 @@ class CHokuyoURG : public C2DRangeFinderAbstract /** The motor speed (default=600rpm) */ int m_motorSpeed_rpm{0}; /** The sensor 6D pose: */ - poses::CPose3D m_sensorPose; + poses::CPose3D m_sensorPose{0, 0, 0, 0, 0, 0}; /** Auxiliary buffer for readings */ mrpt::containers::circular_buffer m_rx_buffer; @@ -259,11 +259,11 @@ class CHokuyoURG : public C2DRangeFinderAbstract /** If set to non-empty, the serial port will be attempted to be opened * automatically when this class is first used to request data from the * laser. */ - std::string m_com_port; + std::string m_com_port{}; /** If set to non-empty and m_port_dir too, the program will try to connect * to a Hokuyo using Ethernet communication */ - std::string m_ip_dir; + std::string m_ip_dir{}; /** If set to non-empty and m_ip_dir too, the program will try to connect to * a Hokuyo using Ethernet communication */ unsigned int m_port_dir{10940}; diff --git a/libs/hwdrivers/src/C2DRangeFinderAbstract.cpp b/libs/hwdrivers/src/C2DRangeFinderAbstract.cpp index 11fd1a73d2..9aa29f2b2e 100644 --- a/libs/hwdrivers/src/C2DRangeFinderAbstract.cpp +++ b/libs/hwdrivers/src/C2DRangeFinderAbstract.cpp @@ -85,6 +85,33 @@ void C2DRangeFinderAbstract::doProcess() } } +void C2DRangeFinderAbstract::internal_notifyGoodScanNow() +{ + const auto new_t = mrpt::system::now(); + + if (m_last_good_scan != INVALID_TIMESTAMP) + { + m_estimated_scan_period = + 0.9 * m_estimated_scan_period + + 0.1 * mrpt::system::timeDifference(m_last_good_scan, new_t); + } + m_last_good_scan = new_t; + m_failure_waiting_scan_counter = 0; +} + +// Returns true if ok, false if this seems to be an error +bool C2DRangeFinderAbstract::internal_notifyNoScanReceived() +{ + const double dt = + mrpt::system::timeDifference(m_last_good_scan, mrpt::system::now()); + + if (dt > 1.50 * m_estimated_scan_period) + if (++m_failure_waiting_scan_counter >= m_max_missed_scan_failures) + return false; + + return true; +} + /*------------------------------------------------------------- loadExclusionAreas -------------------------------------------------------------*/ @@ -156,6 +183,10 @@ void C2DRangeFinderAbstract::loadCommonParams( else break; } + + // Max. missed scan failures: + m_max_missed_scan_failures = configSource.read_int( + iniSection, "maxMissedScansToDeclareError", m_max_missed_scan_failures); } /*------------------------------------------------------------- diff --git a/libs/hwdrivers/src/CHokuyoURG.cpp b/libs/hwdrivers/src/CHokuyoURG.cpp index c2789a7304..b927e657ee 100644 --- a/libs/hwdrivers/src/CHokuyoURG.cpp +++ b/libs/hwdrivers/src/CHokuyoURG.cpp @@ -27,16 +27,7 @@ using namespace std; const int MINIMUM_PACKETS_TO_SET_TIMESTAMP_REFERENCE = 10; -CHokuyoURG::CHokuyoURG() - : m_sensorPose(0, 0, 0), - m_rx_buffer(40000), - - m_com_port(""), - m_ip_dir("") - -{ - m_sensorLabel = "Hokuyo"; -} +CHokuyoURG::CHokuyoURG() : m_rx_buffer(40000) { m_sensorLabel = "Hokuyo"; } CHokuyoURG::~CHokuyoURG() { @@ -93,6 +84,13 @@ void CHokuyoURG::doProcessSimple( m_state = ssWorking; if (!receiveResponse(rcv_status0, rcv_status1)) { + if (!internal_notifyNoScanReceived()) + { + m_state = ssError; + hardwareError = true; + return; + } + // No new data return; } @@ -193,6 +191,7 @@ void CHokuyoURG::doProcessSimple( C2DRangeFinderAbstract::processPreview(outObservation); outThereIsObservation = true; + internal_notifyGoodScanNow(); } /*------------------------------------------------------------- diff --git a/libs/hwdrivers/src/CSickLaserSerial.cpp b/libs/hwdrivers/src/CSickLaserSerial.cpp index 1bee1f266b..40d8aa1fc2 100644 --- a/libs/hwdrivers/src/CSickLaserSerial.cpp +++ b/libs/hwdrivers/src/CSickLaserSerial.cpp @@ -96,7 +96,14 @@ void CSickLaserSerial::doProcessSimple( m_state = ssWorking; // Wait for a scan: - if (!waitContinuousSampleFrame(ranges, LMS_stat, is_mm_mode)) return; + if (!waitContinuousSampleFrame(ranges, LMS_stat, is_mm_mode)) + { + if (!internal_notifyNoScanReceived()) + { + hardwareError = true; + } + return; + } // Yes, we have a new scan: @@ -132,6 +139,7 @@ void CSickLaserSerial::doProcessSimple( C2DRangeFinderAbstract::processPreview(outObservation); outThereIsObservation = true; + internal_notifyGoodScanNow(); } /*-------------------------------------------------------------