Initial commit

Lts/Services/CentralEffectSimulation/Plugins/Basic/simulation_movement.cpp

@@ -0,0 +1,320 @@
+/**
+  * Simulation for Movement
+  * $Id$
+  * $HeadURL$
+  * $Author$
+  * Created by gr, SLSE, 2017-12-13
+  */
+
+#include "simulation_movement.h"
+#include <LtsCoordTrans/lts_coord_trans.h>
+#include <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;
+}
+
+}