Initial commit

Lts/Services/AreaEffectSimulation/object_effector.cpp

@@ -0,0 +1,227 @@
+/**
+ * Object class for ObjectEffector
+ * $Id$
+ * $HeadURL$
+ * $Author$
+ */
+#include "object_effector.h"
+
+#include "main_handler.h"
+#include "EffectorFiringStrategy/abstract_firing_strategy.h"
+#include "EffectorFiringStrategy/firing_strategy_factory.h"
+
+#include <LtsUtilsCore/qpoint3d.h>
+
+namespace AreaEffectSimulation
+{
+
+//-----------------------------------------------------------------------------
+
+ObjectEffector::ObjectEffector(MainHandler *mainHandler, QObject *parent)
+    : QObject(parent)
+    , m_mainHandler(mainHandler)
+    , m_logger(getLogger(QStringLiteral("ObjectEffector")))
+{
+}
+
+bool ObjectEffector::init(int participantId)
+{
+  m_participantId = participantId;
+
+  return true;
+}
+
+AesEffectorState ObjectEffector::currentEffectorState()
+{
+  if (!m_effectorStateMachine)
+    return AesEffectorState::EsNotReadyToFire;
+  return m_effectorStateMachine->getCurrentState();
+}
+
+MainHandler *ObjectEffector::mainHandler() const
+{
+  return m_mainHandler;
+}
+
+int ObjectEffector::aesItemId() const
+{
+  return m_aesItemId;
+}
+
+int ObjectEffector::participantId() const
+{
+  return m_participantId;
+}
+
+QVector<TrajectoryPoint> ObjectEffector::intendendTrajectory() const
+{
+  return m_intendendTrajectory;
+}
+
+EffectorData ObjectEffector::effectorData() const
+{
+  return m_effectorData;
+}
+
+void ObjectEffector::assignAesItemRequest(int aesItemId)
+{
+  DEBUG_LOG(m_logger) << "assignAesItemRequest aesItemId=" << aesItemId;
+  m_aesItemId = aesItemId;
+  // Statemachine mit new anlegen
+  if (m_effectorStateMachine)
+  {
+    delete m_effectorStateMachine;
+    m_effectorStateMachine = nullptr;
+  }
+  m_effectorStateMachine = new EffectorStateMachine(AreaEffectSimulation::AesEffectorState::EsNotReadyToFire, this);
+
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalNotReadyToFire, this, [this]() { emit signalNotReadyToFire(m_participantId, m_aesItemId); });
+
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalReadyToFire, this, [this]() { emit signalReadyToFire(m_participantId, m_aesItemId); });
+
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalReceivedOrder, this, [this]() { emit signalReceivedOrder(m_participantId, m_aesItemId); });
+
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalReportedFired, this, [this]() { emit signalReportedFired(m_participantId, m_aesItemId); });
+
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalFired, this, &ObjectEffector::onEffectorFired);
+
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalNotReadyToFire, this, &ObjectEffector::sendEffectorToDds);
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalReadyToFire, this, &ObjectEffector::sendEffectorToDds);
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalReceivedOrder, this, &ObjectEffector::sendEffectorToDds);
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalReportedFired, this, &ObjectEffector::sendEffectorToDds);
+  connect(m_effectorStateMachine, &EffectorStateMachine::signalFired, this, &ObjectEffector::sendEffectorToDds);
+  connect(m_effectorStateMachine, &EffectorStateMachine::logMessage, this, &ObjectEffector::logMessage);
+
+  m_effectorStateMachine->init();
+
+  // Zuweisung im Objekt veranlassen
+  // ggf. Trajektorie berechnen
+  // publish Trajectory
+}
+
+void ObjectEffector::deleteAssignmentAesItem()
+{
+  DEBUG_LOG(m_logger) << "deleteAssignmentAesItem";
+  if (m_effectorStateMachine)
+  {
+    delete m_effectorStateMachine;
+    m_effectorStateMachine = nullptr;
+  }
+  m_aesItemId = 0;
+}
+
+bool ObjectEffector::reportFireReadinessRequest() // Feuerbereitschaft melden
+{
+  if (!m_effectorStateMachine)
+    return false;
+  return m_effectorStateMachine->reportFireReadiness();
+}
+
+void ObjectEffector::fireOrderRequest() // Feuerbefehl erhalten
+{
+  DEBUG_LOG(m_logger) << "fireOrderRequest";
+  if (!m_effectorStateMachine)
+    return;
+
+  m_effectorStateMachine->fireOrder();
+}
+
+void ObjectEffector::fireRequest() // Feuern
+{
+  DEBUG_LOG(m_logger) << "fireRequest";
+  if (!m_effectorStateMachine)
+    return;
+  m_effectorStateMachine->fire();
+}
+
+bool ObjectEffector::reportFiredRequest() // Abgefeuert melden
+{
+  DEBUG_LOG(m_logger) << "reportFiredRequest";
+  if (!m_effectorStateMachine)
+    return false;
+  return m_effectorStateMachine->reportFired();
+}
+
+void ObjectEffector::reportEmptyChargeRequest(bool emptyCharge) // Munition leer
+{
+  DEBUG_LOG(m_logger) << "reportEmptyChargeRequest";
+  if (!m_effectorStateMachine)
+    return;
+  m_effectorStateMachine->setEmptyCharge(emptyCharge);
+}
+
+bool ObjectEffector::overrideReadynessTimerMinMax(int newMillisecondsMin, int newMillisecondsMax)
+{
+  if (!m_effectorStateMachine)
+    return false;
+  return m_effectorStateMachine->overrideReportFireReadinessMinMax(newMillisecondsMin, newMillisecondsMax);
+}
+
+void ObjectEffector::resetReadynessTimer()
+{
+  if (m_effectorStateMachine)
+    m_effectorStateMachine->resetReportFireReadinessMinMax();
+}
+
+void ObjectEffector::sendEffectorToDds()
+{
+  // Effector Effector;
+  DEBUG_LOG(m_logger) << "sendEffectorToDds NewState= " << m_effectorStateMachine->getCurrentStateStr() << ", participantId=" << m_participantId
+                      << ", aesItemId=" << m_aesItemId;
+  AreaEffectSimulation::Effector effector;
+  effector.id(m_participantId);
+  effector.effectorState(m_effectorStateMachine->getCurrentState());
+  effector.aesItemId(m_aesItemId);
+  effector.participantId(m_participantId);
+  m_mainHandler->dds()->dts()->publish(effector);
+}
+
+void ObjectEffector::onEffectorFired()
+{
+  DEBUG_LOG(m_logger) << "onEffectorFired";
+
+  if (m_firingStrategy == nullptr)
+  {
+    ERROR_LOG(m_logger) << "Not possible to fire. No firing strategy specified.";
+
+    return;
+  }
+
+  m_firingStrategy->doFiring();
+}
+
+// Anhand der EX-L39 Streuungsdaten wird eine Trajektorie für diesen
+// Schuss berechnet.
+// Input: SAA-Format, relativ zum Geschoss in metern und die Zeit relativ zur Abschusszeit
+void ObjectEffector::setIndendedTrajectory(const std::vector<Core::RelativePositionRelativeTime> &geometryRelativeToLaunchOrigin)
+{
+  DEBUG_LOG(m_logger) << "setTrajectory #points=" << geometryRelativeToLaunchOrigin.size();
+  m_intendendTrajectory.clear();
+  for (const Core::RelativePositionRelativeTime &geoPosRelTime : geometryRelativeToLaunchOrigin)
+  {
+    double t = geoPosRelTime.relativeTime();
+    double x = geoPosRelTime.offsetX();
+    double y = geoPosRelTime.offsetY();
+    double z = geoPosRelTime.offsetZ();
+    m_intendendTrajectory.push_back({LtsUtils::QPoint3D(x, y, z), t});
+  }
+}
+
+void ObjectEffector::setEffectorData(const EffectorData &effectorData)
+{
+  DEBUG_LOG(m_logger) << "setEffectorData";
+  m_effectorData = effectorData;
+  setIndendedTrajectory(effectorData.trajectoryRelative());
+  delete m_firingStrategy;
+  m_firingStrategy = FiringStrategyFactory::createFiringStrategy(this);
+  connect(m_firingStrategy, &AbstractFiringStrategy::signalFired,
+          this, &ObjectEffector::signalFired);
+  reportFireReadinessRequest();
+}
+
+void ObjectEffector::logMessage(QString message)
+{
+  DEBUG_LOG(m_logger) << "EffectorStateMachine logMessage=" << message << ", aesItemId=" << m_aesItemId << ", m_participantId=" << m_participantId;
+}
+
+} // namespace AreaEffectSimulation