335 lines
11 KiB
C++
Executable File
335 lines
11 KiB
C++
Executable File
/**
|
|
* Calculate the focal point for units based on it's superiors
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
* Created by Christian Ehrlicher, SLSE, 2017-09-05
|
|
*/
|
|
|
|
|
|
#include "unit_position_calculator.h"
|
|
#include "participant_handler.h"
|
|
#include <QTimer>
|
|
#include <LtsCoordTrans/lts_coord_trans.h>
|
|
#include <LtsUtilsCore/qpoint3d.h>
|
|
#include <LtsUtilsCore/outdated_position.h>
|
|
#include <QPointF>
|
|
#include <QDateTime>
|
|
#include <QtConcurrent>
|
|
|
|
namespace ParticipantManagement
|
|
{
|
|
namespace ParticipantHandling
|
|
{
|
|
UnitPositionCalculator::UnitPositionCalculator(ParticipantHandler *participantHandler, QObject *parent)
|
|
: QObject(parent)
|
|
, m_participantHandler(participantHandler)
|
|
, m_bSomethingChangedSinceLastCalculation(false)
|
|
, m_timer(new QTimer(this))
|
|
, m_bRunning(true)
|
|
, m_logger(getLogger("UnitPositionCalculator"))
|
|
{
|
|
connect(m_timer, &QTimer::timeout, this, &UnitPositionCalculator::calculate);
|
|
m_timer->setSingleShot(true);
|
|
|
|
connect(&m_calculationWatcher, &QFutureWatcher<Result>::finished,
|
|
this, &UnitPositionCalculator::onCalculationDone);
|
|
}
|
|
|
|
UnitPositionCalculator::~UnitPositionCalculator()
|
|
{
|
|
stopCalculation();
|
|
m_calculationWatcher.disconnect(this);
|
|
m_calculationWatcher.cancel();
|
|
m_calculationWatcher.waitForFinished();
|
|
}
|
|
|
|
void UnitPositionCalculator::startCalculation()
|
|
{
|
|
DEBUG_LOG(m_logger) << "Starting calculation timer";
|
|
m_timer->start();
|
|
m_bRunning = true;
|
|
}
|
|
|
|
void UnitPositionCalculator::stopCalculation()
|
|
{
|
|
m_timer->stop();
|
|
m_participantData.clear();
|
|
m_publishedPositionData.clear();
|
|
m_bRunning = false;
|
|
}
|
|
|
|
void UnitPositionCalculator::waitForCalculationFinished()
|
|
{
|
|
if (m_calculationWatcher.isRunning())
|
|
m_calculationWatcher.waitForFinished();
|
|
}
|
|
|
|
|
|
void UnitPositionCalculator::onParticipantDeleted(int participantId)
|
|
{
|
|
DEBUG_LOG(m_logger) << "Participant " << participantId << " deleted";
|
|
auto it = m_participantData.find(participantId);
|
|
if (it == m_participantData.end())
|
|
return;
|
|
m_participantData.erase(it);
|
|
}
|
|
|
|
void UnitPositionCalculator::onPositionChanged(int participantId, int64_t positionTime, const Core::GeoPosition &pos)
|
|
{
|
|
DEBUG_LOG(m_logger) << "Position for participant " << participantId << " changed to "
|
|
<< pos << " (" << positionTime / 1000 << "s)";
|
|
CalculatedUnitPosition &data = getParticipant(participantId);
|
|
if (data.sampleTimeLeader() >= positionTime)
|
|
return;
|
|
|
|
data.sampleTimeCenter(positionTime);
|
|
data.sampleTimeLeader(positionTime);
|
|
data.positionCenter(pos);
|
|
data.positionLeader(pos);
|
|
m_bSomethingChangedSinceLastCalculation = true;
|
|
}
|
|
|
|
void UnitPositionCalculator::onSuperiorChanged(int participantId, int superiorId)
|
|
{
|
|
DEBUG_LOG(m_logger) << "Superior for participant " << participantId << " changed to " << superiorId;
|
|
CalculatedUnitPosition &data = getParticipant(participantId);
|
|
data.superiorId = superiorId;
|
|
m_bSomethingChangedSinceLastCalculation = true;
|
|
}
|
|
|
|
void UnitPositionCalculator::onInferiorChanged(int participantId, const std::vector<int> &inferiorIds)
|
|
{
|
|
if (m_logger.isDebugEnabled())
|
|
{
|
|
QStringList slIds;
|
|
slIds.reserve(inferiorIds.size());
|
|
for (int id : inferiorIds)
|
|
slIds += QString::number(id);
|
|
DEBUG_LOG(m_logger) << "Inferior for participant " << participantId << " changed, size: " << slIds.join(QLatin1Char(' '));
|
|
}
|
|
|
|
CalculatedUnitPosition &data = getParticipant(participantId);
|
|
data.inferiorIds = inferiorIds;
|
|
m_bSomethingChangedSinceLastCalculation = true;
|
|
}
|
|
|
|
UnitPositionCalculator::CalculatedUnitPosition &UnitPositionCalculator::getParticipant(int participantId)
|
|
{
|
|
auto it = m_participantData.find(participantId);
|
|
if (it == m_participantData.end())
|
|
{
|
|
DEBUG_LOG(m_logger) << "No information for participant " << participantId << " - creating dummy data";
|
|
|
|
CalculatedUnitPosition data;
|
|
data.participantId(participantId);
|
|
static const Core::GeoPosition s_invalidPos(std::nan(""), std::nan(""), std::nan(""));
|
|
data.positionCenter(s_invalidPos);
|
|
data.positionLeader(s_invalidPos);
|
|
it = m_participantData.insert({participantId, std::move(data)}).first;
|
|
}
|
|
return it->second;
|
|
}
|
|
|
|
void UnitPositionCalculator::calculate()
|
|
{
|
|
static bool s_bDebugOnce = false;
|
|
// theoretisches Problem: wenn sich keiner bewegt werden die Unit-Symbole nicht aktualisiert wenn
|
|
// dann Teilnehmer out-of-date gehen
|
|
if (!m_bSomethingChangedSinceLastCalculation)
|
|
{
|
|
if (!s_bDebugOnce)
|
|
DEBUG_LOG(m_logger) << "Nothing changed - no calculation needed -> waiting for new data";
|
|
s_bDebugOnce = true;
|
|
m_timer->start(1000);
|
|
return;
|
|
}
|
|
|
|
s_bDebugOnce = false;
|
|
m_bSomethingChangedSinceLastCalculation = false;
|
|
// copy data (before thread starts), can be done better with c++14 (lambda captures)
|
|
std::unordered_map<int, CalculatedUnitPosition> *participantData(new std::unordered_map<int, CalculatedUnitPosition>(m_participantData));
|
|
const qint64 currentTime = m_participantHandler->currentTimeMSec();
|
|
auto cb = [this, participantData, currentTime]() -> Result
|
|
{
|
|
const auto ret = doCalculation(*participantData, currentTime, m_logger);
|
|
delete participantData;
|
|
return ret;
|
|
};
|
|
m_calculationWatcher.setFuture(QtConcurrent::run(cb));
|
|
}
|
|
|
|
UnitPositionCalculator::Result UnitPositionCalculator::doCalculation(std::unordered_map<int, CalculatedUnitPosition> &participantData,
|
|
qint64 currentTime,
|
|
NGL::INGLLogger &logger)
|
|
{
|
|
DEBUG_LOG(logger) << "Starting calculation loop";
|
|
|
|
Result result;
|
|
result.reserve(participantData.size());
|
|
|
|
// search all top-level units
|
|
std::vector<int> topLevelIds;
|
|
topLevelIds.reserve(10);
|
|
for (const auto &data : participantData)
|
|
{
|
|
const int superiorId = data.second.superiorId;
|
|
const int participantId = data.second.participantId();
|
|
DEBUG_LOG(logger) << "Participant " << participantId << " has superior " << superiorId;
|
|
if (superiorId == 0 || participantData.find(superiorId) == participantData.end())
|
|
{
|
|
DEBUG_LOG(logger) << "Participant " << participantId << " is a toplevel participant";
|
|
topLevelIds.push_back(data.second.participantId());
|
|
}
|
|
}
|
|
// TODO: check for correct (non-cyclic) hierarchy?
|
|
|
|
for (const int topLevelId : topLevelIds)
|
|
{
|
|
DEBUG_LOG(logger) << "Calculating positions for toplevel participant " << topLevelId;
|
|
calculatePosition(topLevelId, participantData, currentTime, 1, logger, result);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
void UnitPositionCalculator::calculatePosition(int participantId,
|
|
std::unordered_map<int, CalculatedUnitPosition> &participantData,
|
|
qint64 currentTime,
|
|
int level,
|
|
NGL::INGLLogger &logger,
|
|
Result &result)
|
|
{
|
|
static const Core::GeoPosition s_invalidPos(std::nan(""), std::nan(""), std::nan(""));
|
|
|
|
const QByteArray dbgLvlSpaces(level, ' ');
|
|
auto it = participantData.find(participantId);
|
|
if (it == participantData.end())
|
|
{
|
|
WARNING_LOG(logger) << "Participant " << participantId << " not found for unit position calculation!";
|
|
return;
|
|
}
|
|
CalculatedUnitPosition &data = it->second;
|
|
// found leaf -> return
|
|
if (data.inferiorIds.size() == 0)
|
|
{
|
|
DEBUG_LOG(logger) << dbgLvlSpaces << "Participant " << participantId
|
|
<< " is a leaf: " << data.positionCenter()
|
|
<< " (" << data.sampleTimeCenter() / 1000 << "s)";
|
|
return;
|
|
}
|
|
|
|
data.positionCenter(s_invalidPos);
|
|
data.positionLeader(s_invalidPos);
|
|
|
|
// calculate position for inferiors
|
|
DEBUG_LOG(logger) << dbgLvlSpaces << "Participant " << participantId << ": Start calculating child positions";
|
|
for (const int inferiorId : data.inferiorIds)
|
|
{
|
|
DEBUG_LOG(logger) << dbgLvlSpaces << "Participant " << participantId << ": calculating child position for " << inferiorId;
|
|
calculatePosition(inferiorId, participantData, currentTime, level + 1, logger, result);
|
|
}
|
|
DEBUG_LOG(logger) << dbgLvlSpaces << "Participant " << participantId << ": Finished calculating child positions";
|
|
|
|
// now calc own position
|
|
int utmZone = -1;
|
|
int childCount = 0;
|
|
LtsUtils::QPoint3D pos;
|
|
qint64 childPositionTime = 0;
|
|
bool bFirst = true;
|
|
for (const int inferiorId : data.inferiorIds)
|
|
{
|
|
auto itChild = participantData.find(inferiorId);
|
|
// Warning was already given in loop before
|
|
if (itChild == participantData.end())
|
|
{
|
|
WARNING_LOG(logger) << dbgLvlSpaces << "Child " << inferiorId << ": could not find data";
|
|
continue;
|
|
}
|
|
|
|
const CalculatedUnitPosition &child = itChild->second;
|
|
// older than 10 minutes -> ignore
|
|
if (LtsUtils::isPositionTimeOutdated(currentTime, child.sampleTimeCenter()))
|
|
{
|
|
DEBUG_LOG(logger) << dbgLvlSpaces << "Child " << inferiorId << ": position out-of-date - ignoring";
|
|
continue;
|
|
}
|
|
|
|
// invalid position -> ignore
|
|
if (std::isnan(child.positionCenter().lon()) ||
|
|
std::isnan(child.positionCenter().lat()))
|
|
{
|
|
DEBUG_LOG(logger) << dbgLvlSpaces << "Child " << inferiorId << ": has invalid position - ignoring";
|
|
continue;
|
|
}
|
|
|
|
if (bFirst)
|
|
{
|
|
bFirst = false;
|
|
data.positionLeader(child.positionLeader());
|
|
data.sampleTimeLeader(child.sampleTimeLeader());
|
|
}
|
|
|
|
const auto utm = LtsCoordTrans::UtmCoordinate::fromGeographic(child.positionCenter().lon(),
|
|
child.positionCenter().lat(),
|
|
utmZone);
|
|
if (utmZone == -1)
|
|
utmZone = utm.zone;
|
|
|
|
pos += LtsUtils::QPoint3D(utm.x, utm.y, child.positionCenter().z());
|
|
if (child.sampleTimeCenter() > childPositionTime)
|
|
childPositionTime = child.sampleTimeCenter();
|
|
|
|
++childCount;
|
|
}
|
|
|
|
data.sampleTimeCenter(childPositionTime);
|
|
if (childCount > 0)
|
|
{
|
|
pos /= childCount;
|
|
const LtsCoordTrans::UtmCoordinate utm(utmZone, pos.x(), pos.y(),true);
|
|
const QPointF lonLat = utm.geographic();
|
|
data.positionCenter(Core::GeoPosition(lonLat.x(), lonLat.y(), pos.z()));
|
|
}
|
|
DEBUG_LOG(logger) << dbgLvlSpaces
|
|
<< "Participant " << participantId
|
|
<< ": Mean position: " << data.positionCenter()
|
|
<< " #relavant children: " << childCount
|
|
<< " (" << data.sampleTimeCenter() / 1000 << "s)";
|
|
|
|
result.push_back(std::move(data));
|
|
}
|
|
|
|
void UnitPositionCalculator::onCalculationDone()
|
|
{
|
|
const Result &calculatedPositions = m_calculationWatcher.future().result();
|
|
|
|
auto dts = m_participantHandler->dts();
|
|
for (const auto &pos : calculatedPositions)
|
|
{
|
|
auto it = m_publishedPositionData.find(pos.participantId());
|
|
if (it != m_publishedPositionData.end())
|
|
{
|
|
ParticipantManagement::UnitPosition &up = it->second;
|
|
DEBUG_LOG(m_logger) << "old: " << up << ", new: " << pos << ", equal: " << (up == pos);
|
|
if (up == pos)
|
|
continue;
|
|
// nan == nan is always false, have to check it on our own
|
|
if (std::isnan(up.positionCenter().lat()) && std::isnan(pos.positionCenter().lat()))
|
|
continue;
|
|
up = pos;
|
|
}
|
|
else
|
|
{
|
|
m_publishedPositionData.insert(std::make_pair(pos.participantId(), pos));
|
|
}
|
|
dts->publish(pos);
|
|
}
|
|
|
|
m_timer->start(1000);
|
|
}
|
|
|
|
|
|
} // namespace ParticipantHandling
|
|
} // namespace ParticipantManagement
|