Skip to content
Permalink
Browse files

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

  • Loading branch information...
jlblancoc committed Jun 29, 2019
1 parent 606c7e3 commit c2324ab5deb586e76f9f4ed7e13f94e0cd0033f1
@@ -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
@@ -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};
@@ -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);
}

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

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

/*-------------------------------------------------------------

0 comments on commit c2324ab

Please sign in to comment.
You can’t perform that action at this time.