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";
+    }
+
+
+
+}