265 lines
8.6 KiB
C++
Executable File
265 lines
8.6 KiB
C++
Executable File
/**
|
|
* 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
|