Commit e88569f7 authored by Thiago Santini's avatar Thiago Santini

Fixes Synchronizer bug

If higher priority trigger had been open, it's queue would never be empty.
Thus, if that triggers is turned off, next trigger would never kick in.
parent 971fb7de
......@@ -21,58 +21,50 @@ Synchronizer::Synchronizer(QObject* parent)
this, SLOT(synchronize()));
timer->start(userSynchronizeMs);
}
}
Synchronizer::~Synchronizer()
{
emptyEyeData.timestamp = -1;
emptyFieldData.timestamp = -1;
}
template <class T>
void Synchronizer::updateList(std::list<T>& dataList, T& data)
void Synchronizer::updateDeque(std::deque<T>& dataDeque, T& data)
{
dataList.emplace_back(data);
while (dataList.back().timestamp - dataList.front().timestamp > maxAgeMs)
dataList.pop_front();
dataDeque.emplace_back(data);
while (dataDeque.back().timestamp - dataDeque.front().timestamp > maxAgeMs)
dataDeque.pop_front();
updated = true;
}
void Synchronizer::newRightEyeData(EyeData eyeData)
{
updateList(rEyeList, eyeData);
if (!rEyeList.empty())
synchronize(eyeData.timestamp);
updateDeque(rEyeDeque, eyeData);
synchronize(rEyeDeque.back().timestamp, Trigger::RIGHT_EYE);
}
void Synchronizer::newLeftEyeData(EyeData eyeData)
{
updateList(lEyeList, eyeData);
if (rEyeList.empty())
synchronize(eyeData.timestamp);
updateDeque(lEyeDeque, eyeData);
synchronize(lEyeDeque.back().timestamp, Trigger::LEFT_EYE);
}
void Synchronizer::newFieldData(FieldData fieldData)
{
updateList(fieldList, fieldData);
if (rEyeList.empty() && lEyeList.empty())
synchronize(fieldData.timestamp);
updateDeque(fieldDeque, fieldData);
synchronize(fieldDeque.back().timestamp, Trigger::FIELD_EYE);
}
template <class T>
T* Synchronizer::getClosestInTime(Timestamp timestamp, list<T>& dataList)
T* Synchronizer::getClosestInTime(Timestamp timestamp, deque<T>& dataDeque)
{
T* closest = nullptr;
if (dataList.empty())
if (dataDeque.empty())
return closest;
unsigned int minimum = UINT_MAX;
for (auto it = dataList.begin(); it != dataList.end(); ++it) {
unsigned int diff = abs(timestamp - it->timestamp);
Timestamp minimum = std::numeric_limits<Timestamp>::max();
for (auto& data : dataDeque) {
unsigned int diff = abs(timestamp - data.timestamp);
if (diff < minimum) {
minimum = diff;
closest = &(*it);
closest = &data;
}
}
......@@ -82,14 +74,31 @@ T* Synchronizer::getClosestInTime(Timestamp timestamp, list<T>& dataList)
return closest;
}
void Synchronizer::synchronize(Timestamp timestamp)
bool Synchronizer::shouldSynchronize(Timestamp timestamp, Trigger trigger)
{
switch (trigger) {
case Trigger::FIELD_EYE:
if (!lEyeDeque.empty())
if (timestamp - lEyeDeque.back().timestamp < maxAgeMs)
return false;
case Trigger::LEFT_EYE:
if (!rEyeDeque.empty())
if (timestamp - rEyeDeque.back().timestamp < maxAgeMs)
return false;
case Trigger::RIGHT_EYE:
default:
return true;
}
}
void Synchronizer::synchronize(Timestamp timestamp, Trigger trigger)
{
if (!updated)
return;
updated = false;
if (timestamp == 0)
timestamp = ns2ms(gTimer.nsecsElapsed());
if (!shouldSynchronize(timestamp, trigger))
return;
if (timestamp == maxTimestamp) // nothing recent was available
return;
......@@ -100,15 +109,15 @@ void Synchronizer::synchronize(Timestamp timestamp)
lastSynchronization = timestamp;
FieldData* field = getClosestInTime<FieldData>(timestamp, fieldList);
EyeData* lEye = getClosestInTime<EyeData>(timestamp, lEyeList);
EyeData* rEye = getClosestInTime<EyeData>(timestamp, rEyeList);
FieldData* field = getClosestInTime<FieldData>(timestamp, fieldDeque);
EyeData* lEye = getClosestInTime<EyeData>(timestamp, lEyeDeque);
EyeData* rEye = getClosestInTime<EyeData>(timestamp, rEyeDeque);
DataTuple dataTuple(
timestamp,
(lEye ? *lEye : EyeData()),
(rEye ? *rEye : EyeData()),
(field ? *field : FieldData()));
(lEye ? *lEye : emptyEyeData),
(rEye ? *rEye : emptyEyeData),
(field ? *field : emptyFieldData));
emit newData(dataTuple);
}
#ifndef SYNCHRONIZER_H
#define SYNCHRONIZER_H
#include <list>
#include <deque>
#include <QObject>
#include <QSize>
......@@ -15,7 +15,6 @@ class Synchronizer : public QObject {
public:
explicit Synchronizer(QObject* parent = nullptr);
~Synchronizer();
signals:
void newData(DataTuple dataTuple);
......@@ -26,22 +25,30 @@ public slots:
void newFieldData(FieldData fieldData);
private:
enum class Trigger {
RIGHT_EYE = 0,
LEFT_EYE = 1,
FIELD_EYE = 2,
};
unsigned int maxAgeMs;
unsigned int safeGuardMs;
std::list<EyeData> lEyeList;
std::list<EyeData> rEyeList;
std::list<FieldData> fieldList;
std::deque<EyeData> lEyeDeque;
std::deque<EyeData> rEyeDeque;
std::deque<FieldData> fieldDeque;
EyeData emptyEyeData;
FieldData emptyFieldData;
bool updated;
Timestamp lastSynchronization;
QTimer* timer;
template <class T>
T* getClosestInTime(Timestamp timestamp, std::list<T>& dataList);
T* getClosestInTime(Timestamp timestamp, std::deque<T>& dataDeque);
template <class T>
void updateList(std::list<T>& dataList, T& data);
private slots:
void synchronize(Timestamp timestamp = 0);
void updateDeque(std::deque<T>& dataDeque, T& data);
bool shouldSynchronize(Timestamp timestamp, Trigger trigger);
void synchronize(Timestamp timestamp, Trigger trigger);
};
#endif // SYNCHRONIZER_H
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment