ADD: updated Sensor class

include/Entities/Sensor.hpp

@@ -1,6 +1,8 @@
 #pragma once
 
+#include "DirectCommunicationClient.hpp"
 #include "SimCore/Messages/SimTrack.hpp"
+#include "WHISPER/InternalUDPListener.hpp"
 #include <WHISPER/InternalUDPService.hpp>
 #include <SimCore/Identifier.hpp>
 
@@ -9,66 +11,68 @@
 #include <SimCore/SimCore.hpp>
 #include <SimCore/Identifier.hpp>
 #include <SimCore/Position.hpp>
+#include <atomic>
 #include <memory>
+#include <string>
 #include <thread>
 
 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<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>> incommingTrackMessages = nullptr;
 
             
-            std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> incommingGroundThruthMessages = nullptr;
-            std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> outgoingGroundThruthMessages = nullptr;
            
-            std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> incommingParentMessages = nullptr;
+            virtual void specificSensorCalculations(std::unique_ptr<SimCore::SimTrack> track) = 0;
-            std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> outgoingParentMessages = nullptr;
 
-            virtual void specificSensorCalculations() = 0;
+            std::shared_ptr<SimCore::SimTrack> OwnShipTrack_ = nullptr;
-            virtual void specificReloadCharacteristicts() = 0;
 
-            std::shared_ptr<SimCore::Position> ownShipPosition_ = nullptr;
+            const SimCore::Identifier OwnID_;
-            SimCore::Identifier OwnID_;
+            const SimCore::Identifier OwnShipID;
 
+
+            SimCore::SensorKinds SensorKind_;
+
+            std::shared_ptr<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>> 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<WHISPER::InternalUDPService> GroundTruthUDPService_ = nullptr;    
+            std::unique_ptr<WHISPER::InternalUDPListener> GroundTruthUDPListener_;
-            std::shared_ptr<WHISPER::InternalUDPService> ParentUDPService_ = nullptr;    
 
-            void groundThruthData();
+            std::unique_ptr<DirectCommunication::DirectCommunicationClient> client_;
-            void parentData();
-
-            void SensorCalculations();
-            void ReloadCharacteristicts();
-
-            std::atomic<bool> stopReceivingGroundThruth = false;
-            std::atomic<bool> ReceivingGroundThruthIsRunnung = false;
-
-            std::atomic<bool> stopsendCalculatedData = false;
-            std::atomic<bool> sendCalculatedDataIsRunnung = false;
-
-            std::atomic<bool> stopCalculationData = false;
-            std::atomic<bool> CalculationIsRunnung = false;
 
 
-            std::thread receiveGroundTruthThread;
+            std::thread UpdateOwnShipThread_;
-            std::thread sendCalculatedDataThread;
 
-            std::thread sensorCalculationThread;
+            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<SimCore::SimTrack> ownShipTracK);
 
 
     };

src/Entities/Sensor.cpp

@@ -1,30 +1,49 @@
 #include <SimCore/Position.hpp>
+#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 <Entities/Sensor.hpp>
 #include <memory>
+#include <thread>
 #include <utility>
 
 namespace Entities {
       
-   Sensor::Sensor(SimCore::Identifier OwnID, SimCore::Identifier ParentID, SimCore::SensorKinds SensorKind,std::uint32_t GroundTruthPort, std::uint32_t ParentPort,std::string ParentIPAddress):
+   Sensor::Sensor(
-    OwnID_(OwnID),ParentID_(ParentID),SensorKind_(SensorKind),GroundTruthPort_(GroundTruthPort),ParentPort_(ParentPort),ParentIPAddress_(ParentIPAddress)
+    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)<<SimCore::UtilFunctions::implode(ip,'.');
-        incommingGroundThruthMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
-        outgoingGroundThruthMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
       
-        incommingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
+        recognisedTracks_ =  std::make_shared<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>>();
-        outgoingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
 
-        incommingTrackMessages =  std::make_shared<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>>();
 
+        GroundTruthUDPListener_ = std::make_unique<WHISPER::InternalUDPListener>(GroundTruthAddr_,GroundTruthPort_);
+
+        GroundTruthUDPListener_->registerMessageCallback(std::bind(&Sensor::handleGroundThruthMessage,this,std::placeholders::_1));
+        GroundTruthUDPListener_->connect();
+        GroundTruthUDPListener_->subscribe(WHISPER::MsgTopics::TRACK);
+    
+        client_ = std::make_unique<DirectCommunication::DirectCommunicationClient>(ParentPort_,ParentIPAddress_);
+        client_->registerMessageCallback(std::bind(&Sensor::handlServerMessages,this,std::placeholders::_1));
+        
+        client_->sendMessage("Hello Server");
 
         // GroundTruthUDPService_ =  std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,GroundTruthPort_,SimCore::UtilFunctions::implode(ip,'.'),ownIP);    
         // ParentUDPService_ =  std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,ParentPort,ParentIPAddress_,ownIP);    
@@ -33,196 +52,158 @@ 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;
+        this->stopOwnShipUpdater_ = false;
-    sendCalculatedDataThread = std::thread(&Sensor::parentData,this);
+        UpdateOwnShipThread_ = std::thread(&Sensor::updateOwnShipFunction,this);
       
-    stopCalculationData = false;
-    sensorCalculationThread = std::thread(&Sensor::SensorCalculations,this);
     }
 
     void Sensor::stop() {
 
+        this->stopOwnShipUpdater_ = true;
+        UpdateOwnShipThread_.join();
 
-        while (ReceivingGroundThruthIsRunnung == true ) {
+        GroundTruthUDPListener_->stop();
-            stopReceivingGroundThruth = true;
+        GroundTruthUDPListener_.reset();
-            LOG_S(INFO)<<"waiting for groundthruth thread thread";
-            if (receiveGroundTruthThread.joinable() == true ) {
-                    receiveGroundTruthThread.join();
-            }
-        }
 
-        while (sendCalculatedDataIsRunnung == true ) {
-            stopsendCalculatedData = true;
-            LOG_S(INFO)<<"waiting for parent sending thread thread";
-            if (sendCalculatedDataThread.joinable() == true ) {
-                    sendCalculatedDataThread.join();
-            }
-        }
 
-        while (CalculationIsRunnung == true ) {
+        client_->disconnect();
-            stopCalculationData = true;
+        client_.reset();
-            LOG_S(INFO)<<"waiting for calculation thread thread";
-              if (sensorCalculationThread.joinable() == true ) {
 
-                    sensorCalculationThread.join();
-              }
-        }
         LOG_S(INFO)<<"all stopped";
 
     }
 
 
-    void Sensor::groundThruthData()
+void Sensor::handlServerMessages(std::string msg)
-    {   
+{
-        this->ReceivingGroundThruthIsRunnung = true;
+
-        GroundTruthUDPService_->connect(incommingGroundThruthMessages);
+    try {
-        GroundTruthUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::TRACK]);
+        WHISPER::Message whisperMsg(msg);
+        // LOG_S(INFO)<<"New Message from TCP Client";
+        // LOG_S(INFO)<<"Message Type is: " << whisperMsg.msgType_;
+
+        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_);
             
-        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<SimCore::SimTrack>(std::move(SimCore::SimTrack::unpack(msg)));
-                  incommingTrackMessages->addElement(GTrack);
-                }
-            }   
-
-            if (outgoingGroundThruthMessages->size() > 0) {
-                 WHISPER::Message msg;
-                 outgoingGroundThruthMessages->get(msg);
-                GroundTruthUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]);
             }
 
+            break;
         }
-        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<SimCore::Position>(
-                            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;
-                    }
-                    }
-
-
-
-                                   
-                }   
-                
-            
-
-
-            if (outgoingParentMessages->size() > 0) {
-                WHISPER::Message msg;
-                outgoingParentMessages->get(msg);
-                ParentUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]);
-            }
             
         }
 
-        this->sendCalculatedDataIsRunnung = false;
+
+
+    } catch (std::exception &e) {
+    LOG_S(ERROR)<<e.what();
+
     }
 
+}
+void Sensor::HandleOrders(WHISPER::Message WHmsg)
+{
+    Orders::OrderType type = Orders::Order::getType(WHmsg);
+    switch (type) {
+       case Orders::HOLD_ORDER : 
+       {
 
-    void Sensor::SensorCalculations()
+       break;
+       }
+       case Orders::MOVE_ORDER : 
+       {
+       
+       break;
+       }
+       case Orders::ENGAGE_ORDER :
+       {
+
+       break; 
+       }
+       case Orders::SYSTEM_STATE_ORDER : 
+       {
+
+        break;
+       }
+       case Orders::UNKNOWN:
+         break;
+       }
+}
+
+void Sensor::setupOwnShip(std::shared_ptr<SimCore::SimTrack> ownShipTrack)
+{
+     if(!ownShipTrack->getPosition().isValid())
     {
-        CalculationIsRunnung = true;
+        return;
-        specificSensorCalculations();
+    }
 
-        while (!stopCalculationData) {
+   OwnShipTrack_ = ownShipTrack;
-            specificSensorCalculations();
+//    LOG_S(INFO)<<"Own Ship Received";
-            std::this_thread::sleep_for(std::chrono::milliseconds(500));
+}
 
+
+void Sensor::updateOwnShipFunction()
+{
+    while (stopOwnShipUpdater_ == false) 
+    {
+        if (recognisedTracks_->size() > 0) 
+        {   
+            std::shared_ptr<SimCore::SimTrack> track; 
+            recognisedTracks_->get(track);
+            if (track != nullptr) 
+            {   
+                LOG_S(INFO)<<"updated Tracklist";
+                client_->sendMessage(track->buildMessage().serialize());
+            }
         }
-        LOG_S(INFO)<<"calculation is stopt";
 
-        CalculationIsRunnung = false;
+        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+
     }
+}
 
-    void Sensor::ReloadCharacteristicts()
+
+void Sensor::handleGroundThruthMessage(std::string msg)
+{
+    // LOG_S(INFO)<<"message received";
+    auto track = SimCore::SimTrack::unpack(msg);
+    if (track == nullptr) 
     {
-
+        return;
-        specificReloadCharacteristicts();
     }
+    if (track->getIdentifier() == OwnShipID) {
+        return;
+    }
+    // LOG_S(INFO)<<"calling specific Sensor Calc";
+    specificSensorCalculations(std::move(track));
+
+
+}
+
+   
+
+