diff --git a/include/Entities/Sensor.hpp b/include/Entities/Sensor.hpp index d948a9b..8a0e1f9 100644 --- a/include/Entities/Sensor.hpp +++ b/include/Entities/Sensor.hpp @@ -1,6 +1,8 @@ #pragma once +#include "DirectCommunicationClient.hpp" #include "SimCore/Messages/SimTrack.hpp" +#include "WHISPER/InternalUDPListener.hpp" #include #include @@ -9,66 +11,68 @@ #include #include #include +#include #include +#include #include namespace Entities { class Sensor { public: - Sensor(SimCore::Identifier OwnID, SimCore::Identifier ParentID, SimCore::SensorKinds SensorKind,std::uint32_t GroundTruthPort, std::uint32_t ParentPort,std::string ParentIPAddress); - ~Sensor(); + Sensor( + SimCore::Identifier OwnID, + SimCore::Identifier OwnShipID, + SimCore::SensorKinds SensorKind, + std::string GroundTruthAddress, + std::uint32_t GroundTruthPort, + std::uint32_t ParentPort, + std::string ParentIPAddress); + void start(); void stop(); protected: - std::shared_ptr>> incommingTrackMessages = nullptr; + + + + virtual void specificSensorCalculations(std::unique_ptr track) = 0; + + std::shared_ptr OwnShipTrack_ = nullptr; + + const SimCore::Identifier OwnID_; + const SimCore::Identifier OwnShipID; - std::shared_ptr> incommingGroundThruthMessages = nullptr; - std::shared_ptr> outgoingGroundThruthMessages = nullptr; - - std::shared_ptr> incommingParentMessages = nullptr; - std::shared_ptr> outgoingParentMessages = nullptr; - - virtual void specificSensorCalculations() = 0; - virtual void specificReloadCharacteristicts() = 0; - - std::shared_ptr ownShipPosition_ = nullptr; - SimCore::Identifier OwnID_; + SimCore::SensorKinds SensorKind_; + std::shared_ptr>> recognisedTracks_ = nullptr; private: - SimCore::Identifier ParentID_; - SimCore::SensorKinds SensorKind_; std::uint32_t GroundTruthPort_; + std::string GroundTruthAddr_; std::uint32_t ParentPort_; std::string ParentIPAddress_; - std::shared_ptr GroundTruthUDPService_ = nullptr; - std::shared_ptr ParentUDPService_ = nullptr; + std::unique_ptr GroundTruthUDPListener_; - void groundThruthData(); - void parentData(); - - void SensorCalculations(); - void ReloadCharacteristicts(); - - std::atomic stopReceivingGroundThruth = false; - std::atomic ReceivingGroundThruthIsRunnung = false; - - std::atomic stopsendCalculatedData = false; - std::atomic sendCalculatedDataIsRunnung = false; - - std::atomic stopCalculationData = false; - std::atomic CalculationIsRunnung = false; + std::unique_ptr client_; - std::thread receiveGroundTruthThread; - std::thread sendCalculatedDataThread; - - std::thread sensorCalculationThread; + std::thread UpdateOwnShipThread_; + std::atomic_bool stopOwnShipUpdater_ = false; + + void handleGroundThruthMessage(std::string msg); + + void updateOwnShipFunction(); + + + void handlServerMessages(std::string msg); + + void HandleOrders(WHISPER::Message WHmsg); + + void setupOwnShip(std::shared_ptr ownShipTracK); }; diff --git a/libs/SimCore b/libs/SimCore index 49dd5e2..6845716 160000 --- a/libs/SimCore +++ b/libs/SimCore @@ -1 +1 @@ -Subproject commit 49dd5e295ff6db3ee6f416ffb66855d0ac6b7232 +Subproject commit 68457163959d0851c74a6ee6fb8d69089b16e1b4 diff --git a/src/Entities/Sensor.cpp b/src/Entities/Sensor.cpp index bcd2904..902ebff 100644 --- a/src/Entities/Sensor.cpp +++ b/src/Entities/Sensor.cpp @@ -1,30 +1,49 @@ #include +#include "DirectCommunicationClient.hpp" +#include "Orders/Order.hpp" #include "SimCore/Messages/SimTrack.hpp" #include "SimCore/SimCore.hpp" #include "SimCore/UtilFunctions.hpp" +#include "WHISPER/InternalUDPListener.hpp" #include "WHISPER/Messages/Message.hpp" #include #include +#include #include namespace Entities { - Sensor::Sensor(SimCore::Identifier OwnID, SimCore::Identifier ParentID, SimCore::SensorKinds SensorKind,std::uint32_t GroundTruthPort, std::uint32_t ParentPort,std::string ParentIPAddress): - OwnID_(OwnID),ParentID_(ParentID),SensorKind_(SensorKind),GroundTruthPort_(GroundTruthPort),ParentPort_(ParentPort),ParentIPAddress_(ParentIPAddress) + Sensor::Sensor( + SimCore::Identifier OwnID, + SimCore::Identifier OwnShipID, + SimCore::SensorKinds SensorKind, + std::string GroundTruthAddress, + std::uint32_t GroundTruthPort, + std::uint32_t ParentPort, + std::string ParentIPAddress): + OwnID_(OwnID), + OwnShipID(OwnShipID), + SensorKind_(SensorKind), + GroundTruthAddr_(GroundTruthAddress), + GroundTruthPort_(GroundTruthPort), + ParentPort_(ParentPort), + ParentIPAddress_(ParentIPAddress) { - std::string ownIP = SimCore::UtilFunctions::getOwnIP(); - auto ip = SimCore::UtilFunctions::explode(ownIP, '.'); - ip[3] = "255"; - LOG_S(INFO)<>(); - outgoingGroundThruthMessages = std::make_shared>(); + + recognisedTracks_ = std::make_shared>>(); - incommingParentMessages = std::make_shared>(); - outgoingParentMessages = std::make_shared>(); - incommingTrackMessages = std::make_shared>>(); + GroundTruthUDPListener_ = std::make_unique(GroundTruthAddr_,GroundTruthPort_); + + GroundTruthUDPListener_->registerMessageCallback(std::bind(&Sensor::handleGroundThruthMessage,this,std::placeholders::_1)); + GroundTruthUDPListener_->connect(); + GroundTruthUDPListener_->subscribe(WHISPER::MsgTopics::TRACK); + client_ = std::make_unique(ParentPort_,ParentIPAddress_); + client_->registerMessageCallback(std::bind(&Sensor::handlServerMessages,this,std::placeholders::_1)); + + client_->sendMessage("Hello Server"); // GroundTruthUDPService_ = std::make_shared(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,GroundTruthPort_,SimCore::UtilFunctions::implode(ip,'.'),ownIP); // ParentUDPService_ = std::make_shared(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,ParentPort,ParentIPAddress_,ownIP); @@ -33,197 +52,159 @@ namespace Entities { }; - Sensor::~Sensor(){ - // this->stop(); - if (ReceivingGroundThruthIsRunnung && sendCalculatedDataIsRunnung && CalculationIsRunnung) { - this->stop(); - } - - - this->incommingGroundThruthMessages.reset(); - - - this->outgoingGroundThruthMessages.reset(); - - - // GroundTruthUDPService_->disconnect(); - while (!this->GroundTruthUDPService_.unique()) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - this->GroundTruthUDPService_.reset(); - - LOG_S(INFO)<<"groundThruth is closed"; - ParentUDPService_->disconnect(); - while (!this->ParentUDPService_.unique()) { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - this->ParentUDPService_.reset(); - - - LOG_S(INFO)<<"all destructed"; - - } - + void Sensor::start(){ - ReloadCharacteristicts(); - stopReceivingGroundThruth = false; - receiveGroundTruthThread = std::thread(&Sensor::groundThruthData,this); - stopsendCalculatedData = false; - sendCalculatedDataThread = std::thread(&Sensor::parentData,this); - - stopCalculationData = false; - sensorCalculationThread = std::thread(&Sensor::SensorCalculations,this); + this->stopOwnShipUpdater_ = false; + UpdateOwnShipThread_ = std::thread(&Sensor::updateOwnShipFunction,this); + } void Sensor::stop() { + this->stopOwnShipUpdater_ = true; + UpdateOwnShipThread_.join(); - while (ReceivingGroundThruthIsRunnung == true ) { - stopReceivingGroundThruth = true; - LOG_S(INFO)<<"waiting for groundthruth thread thread"; - if (receiveGroundTruthThread.joinable() == true ) { - receiveGroundTruthThread.join(); - } - } + GroundTruthUDPListener_->stop(); + GroundTruthUDPListener_.reset(); - while (sendCalculatedDataIsRunnung == true ) { - stopsendCalculatedData = true; - LOG_S(INFO)<<"waiting for parent sending thread thread"; - if (sendCalculatedDataThread.joinable() == true ) { - sendCalculatedDataThread.join(); - } - } - while (CalculationIsRunnung == true ) { - stopCalculationData = true; - LOG_S(INFO)<<"waiting for calculation thread thread"; - if (sensorCalculationThread.joinable() == true ) { + client_->disconnect(); + client_.reset(); - sensorCalculationThread.join(); - } - } LOG_S(INFO)<<"all stopped"; } - void Sensor::groundThruthData() - { - this->ReceivingGroundThruthIsRunnung = true; - GroundTruthUDPService_->connect(incommingGroundThruthMessages); - GroundTruthUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::TRACK]); +void Sensor::handlServerMessages(std::string msg) +{ - while (stopReceivingGroundThruth == false) { - if (incommingGroundThruthMessages->size() > 0) { - WHISPER::Message msg; - incommingGroundThruthMessages->get(msg); - if (msg.msgType_ == WHISPER::MsgType::GROUND_TRUTH_TRACK) { - auto GTrack = std::make_shared(std::move(SimCore::SimTrack::unpack(msg))); - incommingTrackMessages->addElement(GTrack); - } - } + try { + WHISPER::Message whisperMsg(msg); + // LOG_S(INFO)<<"New Message from TCP Client"; + // LOG_S(INFO)<<"Message Type is: " << whisperMsg.msgType_; - if (outgoingGroundThruthMessages->size() > 0) { - WHISPER::Message msg; - outgoingGroundThruthMessages->get(msg); - GroundTruthUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]); - } - - } - GroundTruthUDPService_->disconnect(); - this->ReceivingGroundThruthIsRunnung = false; - } - - void Sensor::parentData() - { - this->sendCalculatedDataIsRunnung = true; - - ParentUDPService_->connect(incommingParentMessages); - ParentUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::COMMANDS]); - - while (stopsendCalculatedData == false) { - - if (incommingParentMessages->size() > 0) { - WHISPER::Message msg; - incommingParentMessages->get(msg); - LOG_S(INFO)<< "Message received from Parent is" << msg.msgType_; - std::uint32_t type = 0; - - switch (msg.msgType_) { - case WHISPER::MsgType::OWN_TRACK :{ - - auto OwnTrack = SimCore::SimTrack::unpack(msg); - // SimCore::Track OwnTrack(msg.serialize()); - auto tmpPos = OwnTrack.getPosition().getGeocentricPos(); - if (this->ownShipPosition_ == nullptr) { - this->ownShipPosition_ = std::make_shared( - tmpPos[SimCore::GeocentricPosition::X], - tmpPos[SimCore::GeocentricPosition::Y], - tmpPos[SimCore::GeocentricPosition::Z]); - - }else { - this->ownShipPosition_->setGeocentricPos( - tmpPos[SimCore::GeocentricPosition::X], - tmpPos[SimCore::GeocentricPosition::Y], - tmpPos[SimCore::GeocentricPosition::Z]); - } - - break; - } - case WHISPER::MsgType::COMMAND: { - - break; - } - default: { - - break; - } - } - - - - - } + switch (whisperMsg.msgType_) + { + case WHISPER::MsgType::ORDER: + { + LOG_S(INFO)<<"ORDER"; + HandleOrders(whisperMsg); + + + break; + } + case WHISPER::MsgType::SIM_TRACK: + { + + OwnShipTrack_ = SimCore::SimTrack::unpack(msg); + if (OwnShipTrack_ != nullptr) + { + // LOG_S(INFO)<<"own SHip data received"; + setupOwnShip(OwnShipTrack_); - - - if (outgoingParentMessages->size() > 0) { - WHISPER::Message msg; - outgoingParentMessages->get(msg); - ParentUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]); } + break; + } + } - this->sendCalculatedDataIsRunnung = false; + + + } catch (std::exception &e) { + LOG_S(ERROR)< ownShipTrack) +{ + if(!ownShipTrack->getPosition().isValid()) { - - specificReloadCharacteristicts(); + return; } + OwnShipTrack_ = ownShipTrack; +// LOG_S(INFO)<<"Own Ship Received"; +} + + +void Sensor::updateOwnShipFunction() +{ + while (stopOwnShipUpdater_ == false) + { + if (recognisedTracks_->size() > 0) + { + std::shared_ptr track; + recognisedTracks_->get(track); + if (track != nullptr) + { + LOG_S(INFO)<<"updated Tracklist"; + client_->sendMessage(track->buildMessage().serialize()); + } + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + } +} + + +void Sensor::handleGroundThruthMessage(std::string msg) +{ + // LOG_S(INFO)<<"message received"; + auto track = SimCore::SimTrack::unpack(msg); + if (track == nullptr) + { + return; + } + if (track->getIdentifier() == OwnShipID) { + return; + } + // LOG_S(INFO)<<"calling specific Sensor Calc"; + specificSensorCalculations(std::move(track)); + + +} + + + + + } \ No newline at end of file