berrux-dev
598fc2560e
new_docker_build.sh gains Group 12 (212 GUI/Data plugins); fixes LtsUtilsGuiDds visibility (LTO-stripped exports), LtsProjectionWorkstation unity name shadow, 6 unity-off dirs (dup file-scope statics, ddsftc INTERFACE sources) and reconfigures BUILD_FILTERMANAGEMENT/BUILD_PLUGINS_SERVICEMONITOR=ON
321 lines
11 KiB
C++
Executable File
321 lines
11 KiB
C++
Executable File
/**
|
|
* Simulation for Movement
|
|
* $Id$
|
|
* $HeadURL$
|
|
* $Author$
|
|
* Created by gr, SLSE, 2017-12-13
|
|
*/
|
|
|
|
#include "simulation_movement.h"
|
|
#include <LtsCoordTrans/lts_coord_trans.h>
|
|
#include <QtCore/QDateTime>
|
|
|
|
namespace CentralEffectSimulationBasic
|
|
{
|
|
|
|
SimulationMovement::SimulationMovement(const std::shared_ptr<CentralEffectSimulation::DdsHandler> &ddsHandler,
|
|
const std::shared_ptr<CentralEffectSimulation::DatabaseInterface> &db,
|
|
const std::shared_ptr<CentralEffectSimulation::HandlerParticipant> &participantHandler,
|
|
const CentralEffectSimulation::SimulatedMovement &sml,
|
|
QObject *parent)
|
|
: QObject(parent)
|
|
, m_ddsHandler(ddsHandler)
|
|
, m_db(db)
|
|
, m_handlerParticipant(participantHandler)
|
|
, m_moveTime(1)
|
|
, m_actSegment()
|
|
, m_newPos()
|
|
, m_simStart(true)
|
|
, m_zone(-1)
|
|
, m_segTimer(new QTimer(this))
|
|
, m_moveTimer(new QTimer(this))
|
|
, m_simMovement(sml)
|
|
, m_logger(CentralEffectSimulation::getLogger("Plugin.Basic.SimulationMovement"))
|
|
{
|
|
m_segTimer->setSingleShot(true);
|
|
connect(m_segTimer, &QTimer::timeout, this, &SimulationMovement::startNewSegment);
|
|
|
|
m_moveTimer->setSingleShot(true);
|
|
connect(m_moveTimer, &QTimer::timeout, this, &SimulationMovement::move);
|
|
|
|
setSimulatedMovementRequest(sml);
|
|
}
|
|
|
|
SimulationMovement::~SimulationMovement()
|
|
{
|
|
DEBUG_LOG(m_logger) << "Delete SimulatedMovement "
|
|
<< " id=" << m_simMovement.id
|
|
<< " participantId= " << m_simMovement.participantId;
|
|
m_ddsHandler->dtsCes()->removeSimulatedMovementByKey(m_simMovement.id);
|
|
m_segTimer->stop();
|
|
m_moveTimer->stop();
|
|
}
|
|
|
|
void SimulationMovement::setSimulatedMovementRequest(const CentralEffectSimulation::SimulatedMovement &sml)
|
|
{
|
|
int nextPoint = m_simMovement.nextPoint;
|
|
m_simMovement = sml;
|
|
m_simMovement.nextPoint = nextPoint;
|
|
|
|
m_moveTime = 1;
|
|
m_segTimer->stop();
|
|
m_moveTimer->stop();
|
|
|
|
m_points.clear();
|
|
m_segSpeed.clear();
|
|
m_segDelay.clear();
|
|
m_points.reserve(sml.segments.size());
|
|
m_segSpeed.reserve(sml.segments.size());
|
|
m_segDelay.reserve(sml.segments.size());
|
|
|
|
for (const CentralEffectSimulation::SimulatedMovementSegment &point : sml.segments)
|
|
{
|
|
const Core::GeoPosition2D &pointGeo = point.position;
|
|
m_points.push_back(m_zone == -1 ? CentralEffectSimulation::geographic2Utm(pointGeo, &m_zone) : CentralEffectSimulation::geographic2Utm(pointGeo, m_zone));
|
|
DEBUG_LOG(m_logger) << "pointGeo lat=" << pointGeo.lat << " lon=" << pointGeo.lon
|
|
<< " point lat=" << point.position.lat << " lon=" << point.position.lon
|
|
<< " utmPoint=" << CentralEffectSimulation::debug(m_points.last());
|
|
m_segSpeed.append(point.speed);
|
|
m_segDelay.append(point.delay);
|
|
}
|
|
|
|
// Only start simulation in future
|
|
DEBUG_LOG(m_logger) << "m_startTime - QDateTime::currentMSecsSinceEpoch()=" << m_simMovement.startTime - QDateTime::currentMSecsSinceEpoch()
|
|
<< " m_startTime=" << m_simMovement.startTime
|
|
<< " currentTime=" << QDateTime::currentMSecsSinceEpoch()
|
|
<< " m_nextPoint=" << m_simMovement.nextPoint;
|
|
|
|
if (m_simMovement.nextPoint > 0 && m_points.size() > m_simMovement.nextPoint)
|
|
m_newPos = m_points[m_simMovement.nextPoint].toPoint();
|
|
|
|
if (m_simMovement.startTime - QDateTime::currentMSecsSinceEpoch() > 0)
|
|
m_moveTimer->start(m_simMovement.startTime - QDateTime::currentMSecsSinceEpoch());
|
|
else
|
|
{
|
|
DEBUG_LOG(m_logger) << "Service-Start";
|
|
m_moveTimer->start(10); // Service-Start
|
|
}
|
|
}
|
|
|
|
void SimulationMovement::updateNextPointInDb()
|
|
{
|
|
if (m_simMovement.id <= 0)
|
|
{
|
|
WARNING_LOG(m_logger) << "Internal error - id not set!";
|
|
return;
|
|
}
|
|
CentralEffectSimulation::Database::CesSimulatedMovement dbSim = toDb(m_simMovement);
|
|
m_db->update(dbSim);
|
|
}
|
|
|
|
|
|
void SimulationMovement::startNewSegment()
|
|
{
|
|
DEBUG_LOG(m_logger) << "simulate participantId = " << m_simMovement.participantId
|
|
<< " m_nextPoint = " << m_simMovement.nextPoint;
|
|
|
|
m_moveTime = 1;
|
|
if (m_simMovement.nextPoint < m_points.size())
|
|
{
|
|
QPointF p1;
|
|
// Next segment
|
|
const CentralEffectSimulation::HandlerParticipant::CesParticipantData pos = m_handlerParticipant->position(m_simMovement.participantId);
|
|
if (pos.participantId == m_simMovement.participantId)
|
|
{
|
|
p1 = CentralEffectSimulation::geographic2Utm(pos.pos, m_zone);
|
|
DEBUG_LOG(m_logger) << "Next segment ParticipantPosition =" << CentralEffectSimulation::debug(p1);
|
|
}
|
|
else
|
|
{
|
|
if (m_simStart) // Wait for participantPosition
|
|
{
|
|
INFO_LOG(m_logger) << "Wait for participant position participantId =" << m_simMovement.participantId;
|
|
m_segTimer->start(1000);
|
|
return;
|
|
}
|
|
if (m_simMovement.nextPoint > 0)
|
|
{
|
|
p1 = m_points.at(m_simMovement.participantId - 1);
|
|
DEBUG_LOG(m_logger) << "nextPoint>0 (" << m_simMovement.nextPoint << ") ParticipantPosition =" << CentralEffectSimulation::debug(p1);
|
|
}
|
|
else
|
|
{
|
|
p1 = m_points.at(m_points.size() - 1);
|
|
DEBUG_LOG(m_logger) << "nextPoint=" << m_simMovement.nextPoint << " ParticipantPosition =" << CentralEffectSimulation::debug(p1);
|
|
}
|
|
}
|
|
const QPointF &p2 = m_points.at(m_simMovement.nextPoint);
|
|
m_actSegment.setPoints(p1, p2);
|
|
int curDelay = m_segDelay.at(m_simMovement.nextPoint);
|
|
DEBUG_LOG(m_logger) << "New Segment: p1 = " << CentralEffectSimulation::debug(p1) << " p2=" << CentralEffectSimulation::debug(p2)
|
|
<< " delay =" << curDelay;
|
|
m_moveTimer->start(curDelay);
|
|
}
|
|
else
|
|
{
|
|
if (m_simMovement.repeat)
|
|
{
|
|
m_simMovement.nextPoint = 0;
|
|
m_moveTimer->start(m_segDelay.at(m_simMovement.nextPoint));
|
|
}
|
|
else
|
|
m_simMovement.nextPoint = -1; // Invalid value
|
|
}
|
|
}
|
|
|
|
void SimulationMovement::setJump(bool jump)
|
|
{
|
|
DEBUG_LOG(m_logger) << "Jump";
|
|
if (jump)
|
|
{
|
|
m_simMovement.nextPoint = 1;
|
|
m_newPos = m_points.at(0);
|
|
m_actSegment.setPoints(m_newPos,
|
|
m_points.at(m_simMovement.nextPoint));
|
|
m_moveTime = 1;
|
|
m_simStart = false;
|
|
publishNewPos();
|
|
DEBUG_LOG(m_logger) << "Jump done";
|
|
}
|
|
}
|
|
|
|
void SimulationMovement::publishSimulatedMovement()
|
|
{
|
|
DEBUG_LOG(m_logger) << " m_nextPoint= " << m_simMovement.nextPoint;
|
|
m_ddsHandler->dtsCes()->publish(m_simMovement);
|
|
}
|
|
|
|
void SimulationMovement::publishNewPos()
|
|
{
|
|
TrainingAreaBase::EquipmentPosition participantPos;
|
|
|
|
LtsCoordTrans::UtmCoordinate utmPos;
|
|
utmPos.zone = m_zone;
|
|
utmPos.northp = true;
|
|
utmPos.x = m_newPos.x();
|
|
utmPos.y = m_newPos.y();
|
|
const QPointF posGeo = utmPos.geographic();
|
|
DEBUG_LOG(m_logger) << "participantPos zone=" << utmPos.zone
|
|
<< " northp=" << utmPos.northp
|
|
<< " x=" << utmPos.x
|
|
<< " y=" << utmPos.y;
|
|
|
|
const Core::GeoPosition pos(Core::GeoPosition(posGeo.x(), posGeo.y(), 0));
|
|
|
|
participantPos.equipmentNumber = -m_simMovement.participantId; // negative - treat as participantId
|
|
participantPos.sampleTime = m_ddsHandler->currentPartitionTime();
|
|
participantPos.position.position = pos;
|
|
participantPos.position.reportingSource = Core::PositionReportingSource::MovementSimulation;
|
|
|
|
m_ddsHandler->dtsTab()->publish(participantPos);
|
|
|
|
}
|
|
|
|
void SimulationMovement::move()
|
|
{
|
|
DEBUG_LOG(m_logger) << "move";
|
|
if (m_simMovement.nextPoint < 0)
|
|
{
|
|
DEBUG_LOG(m_logger) << "move next point < 0";
|
|
return;
|
|
}
|
|
|
|
const CentralEffectSimulation::HandlerParticipant::CesParticipantData partPos = m_handlerParticipant->position(m_simMovement.participantId);
|
|
const QPointF participantPosUtm = CentralEffectSimulation::geographic2Utm(partPos.pos, m_zone);
|
|
if (partPos.participantId != m_simMovement.participantId)
|
|
{
|
|
DEBUG_LOG(m_logger) << "move no ParticipantId=" << m_simMovement.participantId;
|
|
m_moveTimer->start(1000);
|
|
return;
|
|
}
|
|
if (!partPos.positionValid)
|
|
{
|
|
DEBUG_LOG(m_logger) << "move no ParticipantId=" << m_simMovement.participantId;
|
|
m_moveTimer->start(1000);
|
|
return;
|
|
}
|
|
DEBUG_LOG(m_logger) << " pos=" << CentralEffectSimulation::debug(participantPosUtm);
|
|
|
|
QLineF step = m_actSegment;
|
|
if (m_simStart)
|
|
{
|
|
m_actSegment.setPoints(participantPosUtm,
|
|
m_points.at(m_simMovement.nextPoint));
|
|
step = m_actSegment;
|
|
DEBUG_LOG(m_logger) << "m_simStart set m_actSegment"
|
|
<< " p1: " << CentralEffectSimulation::debug(m_actSegment.p1())
|
|
<< " p2: " << CentralEffectSimulation::debug(m_actSegment.p2());
|
|
m_simStart = false;
|
|
}
|
|
|
|
if (m_segSpeed[m_simMovement.nextPoint] <= 0.0)
|
|
{
|
|
DEBUG_LOG(m_logger) << "move next point < 0";
|
|
return;
|
|
}
|
|
|
|
m_newPos = m_points[m_simMovement.nextPoint];
|
|
step.setLength(m_segSpeed[m_simMovement.nextPoint] * m_moveTime);
|
|
m_moveTime++;
|
|
// ready criterion
|
|
if (step.length() >= m_actSegment.length() + 0.1 ||
|
|
step.length() < 0.1) // 0.1 Magic Number but 10 cm distance to Point is arrived
|
|
{
|
|
DEBUG_LOG(m_logger) << "move ready step.length()=" << step.length()
|
|
<< " m_actSegment.length()=" << m_actSegment.length();
|
|
m_simMovement.nextPoint = m_simMovement.nextPoint + 1;
|
|
if (m_simMovement.nextPoint >= m_points.size())
|
|
{
|
|
if (m_simMovement.repeat)
|
|
m_simMovement.nextPoint = 0;
|
|
else
|
|
{
|
|
m_simMovement.nextPoint = -1; // Invalid value
|
|
return;
|
|
}
|
|
}
|
|
updateNextPointInDb();
|
|
DEBUG_LOG(m_logger) << "ready move participantId = " << m_simMovement.participantId
|
|
<< " m_nextPoint =" << m_simMovement.nextPoint
|
|
<< " newPos =" << m_newPos.x() << "/" << m_newPos.y()
|
|
<< " m_moveTime=" << m_moveTime
|
|
<< " length=" << m_segSpeed[m_simMovement.nextPoint] * m_moveTime
|
|
<< " step.length=" << step.length()
|
|
<< " m_segSpeed[m_nextPoint]=" << m_segSpeed[m_simMovement.nextPoint]
|
|
<< " m_moveTime=" << m_moveTime
|
|
<< " m_actSegment.length=" << m_actSegment.length()
|
|
<< " m_zone=" << m_zone;
|
|
publishNewPos();
|
|
startNewSegment();
|
|
publishSimulatedMovement();
|
|
return;
|
|
}
|
|
m_newPos = step.p2();
|
|
DEBUG_LOG(m_logger) << "move participantId = " << m_simMovement.participantId
|
|
<< " m_nextPoint =" << m_simMovement.nextPoint
|
|
<< " newPos =" << m_newPos.x() << "/" << m_newPos.y()
|
|
<< " m_moveTime=" << m_moveTime
|
|
<< " length=" << m_segSpeed[m_simMovement.nextPoint] * m_moveTime
|
|
<< " step.length=" << step.length()
|
|
<< " m_speed=" << m_segSpeed[m_simMovement.nextPoint]
|
|
<< " m_moveTime=" << m_moveTime
|
|
<< " m_actSegment.length=" << m_actSegment.length()
|
|
<< " m_zone=" << m_zone;
|
|
publishNewPos();
|
|
m_moveTimer->start(1000); // Calculate new position each second
|
|
}
|
|
|
|
CentralEffectSimulation::Database::CesSimulatedMovement SimulationMovement::toDb(const CentralEffectSimulation::SimulatedMovement &ddsSimMovement)
|
|
{
|
|
CentralEffectSimulation::Database::CesSimulatedMovement ret;
|
|
ret.id = ddsSimMovement.id;
|
|
ret.name = QString::fromStdString(ddsSimMovement.name);
|
|
ret.startTime = ddsSimMovement.startTime;
|
|
ret.participantId = ddsSimMovement.participantId;
|
|
ret.repeat = ddsSimMovement.repeat;
|
|
ret.nextpoint = ddsSimMovement.nextPoint;
|
|
return ret;
|
|
}
|
|
|
|
}
|