initial commit

src/Entities/Entity.cpp

@@ -0,0 +1,182 @@
+#include "WHISPER/InternalUDPListener.hpp"
+#include "WHISPER/InternalUDPSender.hpp"
+#include "WHISPER/Messages/Message.hpp"
+#include "WHISPER/Messages/stringData.hpp"
+#include "WHISPER/threadSafeQueue.hpp"
+#include <SimCore/SimCore.hpp>
+#include <SimCore/Templates/Entity.hpp>
+
+#include <chrono>
+#include <memory>
+#include <thread>
+
+
+
+#define calculationPeriode 100
+
+namespace SimCore
+{
+
+    Entity::Entity(const SimCore::Identifier OwnID, 
+                   std::string EnttityName,
+                   WHISPER::SourceType OwnType,
+                   SimCore::Identifier ParentID,
+                   SimCore::EntityKind EntityKind,
+                   std::uint32_t GroundTruthPort, 
+                   std::uint32_t CommandPort,
+                   std::string CommandIPAddress):EntityName_(EnttityName),
+                   ownTrack_(OwnType, OwnID, SimCore::TrackKind::GROUND_TRUTH_TRACK),
+                   ParentID_(ParentID),
+                   EntityKind_(EntityKind),
+                   GroundTruthPort_(GroundTruthPort),
+                   CommandPort_(CommandPort),
+                   CommandIPAddress_(CommandIPAddress)
+
+    {
+    
+    }
+
+    void Entity::start()
+    {   
+
+        stopCommandWorker = false;
+        stopSensorWorker = false;
+        stopTrackWorker = false;
+        stopPhysicsWorker = false;
+
+        threads.emplace_back(std::thread(&Entity::CommandWorker,this));
+        threads.emplace_back(std::thread(&Entity::SensorWorker,this));
+        threads.emplace_back(std::thread(&Entity::TrackWorker,this));
+        threads.emplace_back(std::thread(&Entity::physicsWorker,this));
+
+
+    }
+
+    void Entity::stop()
+    {
+        stopCommandWorker = true;
+        stopSensorWorker = true;
+        stopTrackWorker = true;
+        stopPhysicsWorker = true;       
+        // for (auto &th :threads)
+        //     {
+        //         if (th.joinable()) {
+        //          th.join();
+        //         }
+                
+        //     }
+
+
+    for (std::vector<std::thread>::iterator it = threads.begin(); it != threads.end();)
+    {
+        if (it->joinable()) 
+        {
+            it->join();
+            it = threads.erase(it);
+        }
+       
+    }
+
+        LOG_S(ERROR)<< threads.size();
+
+    
+
+
+    }
+
+    void Entity::physicsWorker()
+    {
+    physicsIsRunning = true;
+
+    while (!stopPhysicsWorker) {
+    auto start =  std::chrono::steady_clock::now();
+    
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(calculationPeriode));   
+
+
+    auto end = std::chrono::steady_clock::now();
+    std::chrono::milliseconds::rep duration =  std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
+    specificPhysicsCalculations(duration);
+    }    
+
+    physicsIsRunning = false;
+    }
+
+
+
+    void Entity::CommandWorker()
+    {
+        auto CommandUDPListener = std::make_shared<WHISPER::InternalUDPListener>(CommandPort_) ;    
+        auto receiverQueue = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
+        CommandUDPListener->connect(receiverQueue);
+
+        
+        auto CommandUDPSender = std::make_shared<WHISPER::InternalUDPSender>(CommandIPAddress_,CommandPort_);    
+
+
+        while (!stopCommandWorker) 
+        {
+            if (receiverQueue->size() > 0) {
+                
+                auto msg =  WHISPER::Message();
+                receiverQueue->get(msg);
+
+                switch (msg.msgType_) {
+                
+                    case WHISPER::MsgType::STRINGDATA :{
+                        WHISPER::StringData stringMsg =  WHISPER::StringData(msg.serialize());
+                        std::string str = stringMsg.data_;
+                        break;
+                    }
+
+                    case WHISPER::MsgType::COMMAND: {
+                        WHISPER::StringData string = WHISPER::StringData(msg.serialize());
+                        break;
+                    }
+                }
+
+            }
+
+
+            LOG_S(INFO)<<"hello from command worker";
+            std::this_thread::sleep_for(std::chrono::milliseconds(900));   
+
+        }
+
+
+    }
+
+    void Entity::SensorWorker()
+    {
+
+
+        while (!stopSensorWorker) 
+        {
+            LOG_S(INFO)<<"hello from sensor worker";
+            std::this_thread::sleep_for(std::chrono::milliseconds(900));   
+
+        }
+
+    }
+
+    void Entity::TrackWorker()
+    {
+        while (!stopTrackWorker) 
+        {
+            LOG_S(INFO)<<"hello from track worker";
+            std::this_thread::sleep_for(std::chrono::milliseconds(900));   
+
+        }
+
+    }
+
+    void Entity::startSensor()
+    {
+        
+        LOG_S(ERROR)<< "starting new pods is not implemented yet";
+    }
+
+
+
+}

src/Entities/Sensor.cpp

@@ -0,0 +1,214 @@
+#include "SimCore/Messages/GroundThruthTrack.hpp"
+#include "SimCore/Messages/Track.hpp"
+#include "SimCore/Position.hpp"
+#include "SimCore/SimCore.hpp"
+#include "SimCore/UtilFunctions.hpp"
+#include "WHISPER/Messages/Message.hpp"
+#include <SimCore/Templates/Sensor.hpp>
+#include <memory>
+
+namespace SimCore {
+      
+   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)
+    {
+
+        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>>();
+        outgoingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
+
+        incommingTrackMessages =  std::make_shared<WHISPER::threadSafeQueue<SimCore::GroundTruthTrack>>();
+    
+
+        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);    
+
+
+    };
+
+
+    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);
+    }
+
+    void Sensor::stop() {
+
+
+        while (ReceivingGroundThruthIsRunnung == true ) {
+            stopReceivingGroundThruth = true;
+            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 ) {
+            stopCalculationData = true;
+            LOG_S(INFO)<<"waiting for calculation thread thread";
+              if (sensorCalculationThread.joinable() == true ) {
+
+                    sensorCalculationThread.join();
+              }
+        }
+        LOG_S(INFO)<<"all stopped";
+
+    }
+
+
+    void Sensor::groundThruthData()
+    {   
+        this->ReceivingGroundThruthIsRunnung = true;
+        GroundTruthUDPService_->connect(incommingGroundThruthMessages);
+        GroundTruthUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::TRACK]);
+
+        while (stopReceivingGroundThruth == false) {
+            if (incommingGroundThruthMessages->size() > 0) {
+                WHISPER::Message msg; 
+                incommingGroundThruthMessages->get(msg);
+                if (msg.msgType_ == WHISPER::MsgType::GROUND_TRUTH_TRACK) {
+                    auto elem = GroundTruthTrack::unpack(msg);
+                //   auto Track = SimCore::Track(msg.serialize());
+                  incommingTrackMessages->addElement(elem);
+                }
+            }   
+
+            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;
+                    
+                    if (msg.msgType_ == WHISPER::MsgType::OWN_TRACK) {
+                        auto OwnTrack = SimCore::GroundTruthTrack::unpack(msg);
+                        // SimCore::Track OwnTrack(msg.serialize());
+                        auto tmpPos = OwnTrack.getPostion().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]);
+                        }
+                    }
+                    
+                }   
+                
+            
+
+
+            if (outgoingParentMessages->size() > 0) {
+                WHISPER::Message msg;
+                outgoingParentMessages->get(msg);
+                ParentUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]);
+            }
+
+        }
+
+        this->sendCalculatedDataIsRunnung = false;
+    }
+
+
+    void Sensor::SensorCalculations()
+    {   
+        CalculationIsRunnung = true;
+        specificSensorCalculations();
+
+        while (!stopCalculationData) {
+            specificSensorCalculations();
+            std::this_thread::sleep_for(std::chrono::milliseconds(500));
+
+        }
+        LOG_S(INFO)<<"calculation is stopt";
+
+        CalculationIsRunnung = false;
+    }
+
+    void Sensor::ReloadCharacteristicts()
+    {
+
+        specificReloadCharacteristicts();
+    }
+
+
+
+}