2D scans: estimate rate, return error if timing out

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

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

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);
 }
 
 /*-------------------------------------------------------------

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();
 }
 
 /*-------------------------------------------------------------

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();
 }
 
 /*-------------------------------------------------------------