Initial commit

Lts/Services/CentralEffectSimulation/Plugins/MissionPlan/ces_mission_plan_handler.cpp

@@ -0,0 +1,264 @@
+/**
+  * Plugin CES Mission Plan
+  * $Id:  $
+  * $HeadURL: svn://slssvn/TDS/branches/product/features/25_03_etb_uas/Lts/Services/CentralEffectSimulation/Plugins/MissionPlan/ces_mission_plan_handler.cpp $
+  * $Author: Kemayou_s $
+  * Created by Steve Kemayou, ESZS, 2025-06-25
+  */
+
+#include "ces_mission_plan_handler.h"
+
+#include <QTimer>
+#include <QDateTime>
+#include <QRandomGenerator>
+
+#include <LtsUtilsCore/dds_bitarray.h>
+#include <LtsCoordTrans/lts_coord_trans.h>
+#include <ProjectDataManagement/pdm_enums.hpp>
+
+namespace dds
+{
+namespace core
+{
+inline uint qHash(ProjectDataManagement::EventType et)
+{
+  return ::qHash(et.underlying());
+}
+}
+}
+
+namespace CES_MISSIONPLAN
+{
+
+
+CesMissionPlanHandler::CesMissionPlanHandler(const std::shared_ptr<CentralEffectSimulation::DdsHandler> &ddsHandler,
+                                               const std::shared_ptr<CentralEffectSimulation::HandlerParticipant> &participantHandler,
+                                               QObject *parent)
+  : QObject(parent)
+  , m_ddsHandler(ddsHandler)
+  , m_handlerParticipant(participantHandler)
+  , m_logger(CentralEffectSimulation::getLogger("Plugin.MissionPlan.CesMissionPlanHandler"))
+{
+} // end of ctor()
+//-----------------------------------------------------------------------------
+
+CesMissionPlanHandler::~CesMissionPlanHandler()
+{
+  qDeleteAll(m_missionPlanObjects);
+} // end of dtor()
+//-----------------------------------------------------------------------------
+
+bool CesMissionPlanHandler::init()
+{
+  // TAB FTC
+  m_ddsHandler->dtcPpm()->enableUAS();
+  m_ddsHandler->dtcPpm()->enableMissionPlan();
+  m_ddsHandler->dtcPpm()->enableParticipantGearEvent();
+  m_ddsHandler->ftcPpm()->enableSetMissionActionFulfilled();
+  m_ddsHandler->ftcAes()->enableExplosionClusterModify();
+  m_ddsHandler->ftcTab()->enableSendUasWaypoint();
+
+  connect (m_ddsHandler->dtcPpm().get(), &ParticipantManagement::Interface::Dtc::uASChanged,
+           this, &CesMissionPlanHandler::onUasChanged);
+
+  connect (m_ddsHandler->dtcPpm().get(), &ParticipantManagement::Interface::Dtc::missionPlanChanged,
+           this, &CesMissionPlanHandler::onMissionPlanChanged);
+
+
+  DEBUG_LOG(m_logger) << " -- MissionPlanHandler::init() finished.";
+  return true;
+}
+
+void CesMissionPlanHandler::updateMissionPlanList(const ParticipantManagement::MissionPlan &data)
+{
+  CesMissionPlanObject *obj = m_missionPlanObjects.value(data.id(), nullptr);
+
+  if (obj)
+  {
+    m_missionPlanObjects[data.id()]->setMissionPlan(data);
+    return;
+  }
+
+  // check if mission start is in the future before proceeding..
+  DEBUG_LOG(m_logger)<<" -- [updateMissionPlanList] new missionPlan : "<<data.name()<< " planId: "<<data.id();
+
+  if (data.missionStart() < m_ddsHandler->currentPartitionTime())
+  {
+    WARNING_LOG(m_logger) << " -- updateMissionPlanList mission startTime ("<<data.missionStart()<<") in the past ignore mission plan id: "<<data.id()
+                          << " name: " << data.name() << " - now: " << m_ddsHandler->currentPartitionTime();
+    return;
+  }
+
+  obj = new CesMissionPlanObject(data, m_ddsHandler, this);
+  connect(obj, &CesMissionPlanObject::ready2SendNextAction, this,
+          &CesMissionPlanHandler::onSendNextMissionAction, Qt::UniqueConnection);
+
+  const int planId = obj->planId();
+  std::function<void(void)> onDeleteObj = [this, planId]()
+  {
+    DEBUG_LOG(m_logger)<<" --  remove ces mission object from container planId: "<<planId;
+    m_missionPlanObjects.remove(planId);
+  };
+  connect(obj, &CesMissionPlanObject::destroyed, this, onDeleteObj);
+
+  m_missionPlanObjects.insert(data.id(), obj);
+}
+
+void CesMissionPlanHandler::onUasChanged(const ParticipantManagement::UAS &data, DataModification modType)
+{
+  Q_UNUSED(modType)
+  DEBUG_LOG(m_logger)<<" -- onUasChanged : uas.id = "<<data.id() <<" participantId = "<<data.participantId();
+  // get mission object of corresponding uas participant
+  // if drone status is manual mode then cancel mission.
+  if (data.uasState() != ProjectDataManagement::UasState_def::Manual && modType != DataModification::eRemoved)
+    return;
+
+  for (auto planObj :  m_missionPlanObjects)
+  {
+    if (planObj->participantId() != data.participantId())
+      continue;
+
+    // if waypoint started and state switched to manual abord mission
+    if (planObj->missionPlan().missionStart() < m_ddsHandler->currentPartitionTime())
+      planObj->abortMission();
+    break;
+  }
+}
+
+void CesMissionPlanHandler::onMissionPlanChanged(const ParticipantManagement::MissionPlan &data, DataModification modType)
+{
+  DEBUG_LOG(m_logger)<<" -- onMissionPlanChanged : missionPlanName: "<<data.name() << ", modType: " << (int)modType;
+  if (data.id() <= 0)
+    return;
+  switch (modType)
+  {
+  case DataModification::eAdded:
+  case DataModification::eModified:
+    updateMissionPlanList(data);
+    break;
+  case DataModification::eRemoved:
+  {
+    auto obj = m_missionPlanObjects.value(data.id(), nullptr);
+    if (!obj)
+      return;
+    delete obj;
+
+    m_missionPlanObjects.remove(data.id());
+  }
+    break;
+  default:
+    break;
+  }
+}
+
+bool CesMissionPlanHandler::isUasInAutoMode(int participantId)
+{
+
+  m_ddsHandler->dtcPpm()->waitForHistoricalDataUAS(3000);
+  const auto uas = m_ddsHandler->dtcPpm()->getUasByParticipantId(participantId, m_ddsHandler->getExerciseId());
+
+  if (uas.id() < 1)
+  {
+    ERROR_LOG(m_logger) << "no uas for pid " << participantId;
+    return false;
+  }
+
+  DEBUG_LOG(m_logger) << "uas with pid " << participantId << " is in mode: " << (int)uas.uasState().underlying();
+
+  return (uas.uasState() == ProjectDataManagement::UasState::Auto);
+}
+
+// stolen from Lts/Plugins/ParticipantManagement/Gui/ParticipantInfo/participant_info_dialog.cpp
+bool CesMissionPlanHandler::isUasCombatReady(int participantId)
+{
+
+  m_ddsHandler->dtcPpm()->waitForHistoricalDataDynamicParticipantData(3000);
+  const auto data = m_ddsHandler->dtcPpm()->dynamicParticipantData(participantId);
+
+  if (data.participantId() == 0)
+  {
+    return false;
+  }
+
+  //Current State
+  DEBUG_LOG(m_logger) << "stateHistory count" << data.stateHistory().size();
+  if (data.stateHistory().empty())
+  {
+    return false;
+  }
+
+  const auto &history = data.stateHistory();
+
+  //LasState is the last "real" state, that has a VBG <=8 (Castastrophic)
+  //It's not the last state received, as there can be other state changes like Sanitation, or NearMiss
+  auto lastState = m_ddsHandler->dtcPpm()->youngestStateInformation(history/*, true*/ /* debug */);
+  decltype(lastState) youngestState;
+  youngestState.stateTime(0);
+  for (const auto &si : history)
+  {
+    DEBUG_LOG(m_logger) << "StateInformation "
+                         << " time=" << si.stateTime()
+                         << " vulnerabilityBits=" << si.vulnerabilityBits()
+                         << " vulnerabilityKindId=" << si.vulnerabilityKindId();
+    if (youngestState.stateTime() < si.stateTime())
+      youngestState = si;
+  }
+
+  return (youngestState.vulnerabilityState() == ProjectDataManagement::VulnerabilityState::CombatReady);
+}
+
+void CesMissionPlanHandler::onSendNextMissionAction(const ParticipantManagement::MissionAction &msa)
+{
+  auto missionPlan = m_missionPlanObjects.value(msa.planId(), nullptr);
+  if (!missionPlan)
+  {
+    DEBUG_LOG(m_logger)<<" -- [SEND NEXT MISSION] Object not found. PlanId: "<<msa.planId();
+    return;
+  }
+
+
+  // send to TAB
+  const auto participantId = missionPlan->m_missionPlan.participantId();
+  const auto participant = m_handlerParticipant->participant(participantId);
+
+  if (!participant.equipmentNumber())
+  {
+    ERROR_LOG(m_logger) << "unable to send next mission action to uav without equipment number";
+    return;
+  }
+
+
+
+
+  if (!isUasInAutoMode(participantId))
+  {
+    ERROR_LOG(m_logger) << "unable to send next mission action to uav that is not in auto mode";
+    //TODO: fulfill all other missions or mark as incomplete
+    return;
+  }
+ 
+
+  if (!isUasCombatReady(participantId))
+  {
+    ERROR_LOG(m_logger) << "unable to send next mission action to uav that is not combat ready";
+    //TODO: fulfill all other missions or mark as incomplete
+    return;
+  }
+
+  const auto eqId = participant.equipmentNumber().value();
+
+  try
+  {
+    // send action to uav via tab - if this doesn't throw, waypoint is now executing
+    m_ddsHandler->ftcTab()->sendUasWaypointRequest(TrainingAreaBase::SendUasWaypointRequest(eqId, participantId, msa.actionNumber(), msa.itemNumber(), msa.planId(), msa.name(), msa.properties()));
+    // PPM will send a change request to the current active action, which in return tell us (updateMissionPlanList) and finally we will start the next action - so we are done here for now
+  }
+  catch (const std::exception &ex)
+  {
+    ERROR_LOG(m_logger) << "unable to send next mission action to uav: " << ex.what();
+    return;
+  }
+}
+
+
+} // namespace CES_MISSIONPLAN