ADD: added Movement calculation and a Tracklist class

include/Entities/Entity.hpp

@@ -2,6 +2,7 @@
 
 
 
+#include "Entities/Movement.hpp"
 #include "SimCore/Messages/GroundThruthTrack.hpp"
 #include "SimCore/Messages/Track.hpp"
 #include "SimCore/Orientation.hpp"
@@ -26,7 +27,7 @@
 
 namespace Entities {
 
-    struct SensorData
+    struct SensorClientData
     { 
         std::string SensorName;
         bool isActive;
@@ -34,7 +35,7 @@ namespace Entities {
         std::shared_ptr<WHISPER::InternalUDPSender> SensorSender;
     };
 
-    struct EffectorData
+    struct EffectorClientData
     { 
         std::string EffectorName;
         bool isActive;
@@ -58,8 +59,7 @@ namespace Entities {
 
             void start();
             void stop();
-        
-        
+
         protected:
 
             std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> incommingCommandMessages = nullptr;
@@ -68,17 +68,13 @@ namespace Entities {
             virtual void specificPhysicsCalculations(std::chrono::milliseconds::rep duration) = 0;
             virtual void specificReloadCharacteristicts() = 0;
 
-            std::shared_ptr<SimCore::Position> ownShipPosition_ = nullptr;
-            std::shared_ptr<SimCore::Orientation> ownShipOrientation_ = nullptr;
-
+            Entities::Movement Movement_;
 
         private:
 
             std::string EntityName_;
 
 
-
-
             SimCore::GroundTruthTrack ownTrack_;
             SimCore::Identifier ParentID_;
             SimCore::EntityKind EntityKind_;
@@ -111,7 +107,7 @@ namespace Entities {
               
             std::shared_ptr<WHISPER::InternalUDPSender> GroundTruthUDPSender_ = nullptr;   
 
-            std::shared_ptr<std::list<Entities::SensorData>> SensorStore_;
+            std::shared_ptr<std::list<Entities::SensorClientData>> SensorStore_;
 
             std::shared_ptr<SimCore::SafeMap<SimCore::Identifier, std::shared_ptr<SimCore::Track>>> Trackstore_;
 

include/Entities/Movement.hpp

@@ -0,0 +1,106 @@
+#pragma once
+
+
+#include <SimCore/Orientation.hpp>
+#include <SimCore/Position.hpp>
+#include <condition_variable>
+#include <cstdint>
+#include <Eigen/Core>
+#include <loguru.hpp>
+
+
+namespace Entities 
+{
+
+       
+
+   
+
+    
+
+  
+
+
+    class Movement
+    {   
+        public:
+            Movement();
+            Movement(SimCore::Position pos);
+            Movement(SimCore::Position pos, double course);
+            Movement(SimCore::Position pos, double course, double speed);
+            Movement(SimCore::Position pos, double course, double speed, double pitch);
+
+            
+
+            // ~Movement();
+
+            void updatePosition(double dt);
+
+
+            /**
+            * @brief returns the current Course representing the direction the front of the entity is pointing 
+            *
+            */
+            double getCourse();
+
+            void setCourse(double course);
+
+            /**
+            * @brief returns the speed in m/s
+            *
+            */
+            double getSpeed();
+
+            void setSpeed(double speed);
+
+            /**
+            * returns the climbing angle of the entity of the entity
+            *
+            */
+            double getPitch();
+
+            void setPitch(double pitch);
+
+            /**
+            * @brief returns the kinematicVector of Entity
+            *
+            */
+            Eigen::Vector3d getKinematicVector();
+
+            SimCore::Position getPosition();
+
+            void setPosition(double lat, double lon, double height = 0);
+
+            void setPosition(SimCore::Position pos);
+
+
+            SimCore::Orientation getownOrientation();
+
+            Eigen::Vector3d getEulerAngles();
+
+
+
+
+
+        private:
+            SimCore::Position ownPosition_;
+            SimCore::Orientation ownOrientation_;
+
+            Eigen::Vector3d kinematicVec_;
+            Eigen::Vector3d accelerationVec_;
+
+
+            double course_;
+            double speed_;
+            double pitch_;
+
+            void calKinematicVector();
+            Eigen::Matrix3d getRotationMatrix(double lat, double lon);
+
+
+            mutable std::mutex mx;
+            std::condition_variable c;
+
+
+    };
+}

include/Entities/Tracklist/Tracklist.hpp

@@ -0,0 +1,62 @@
+#pragma once
+
+
+#include "SimCore/Identifier.hpp"
+#include "SimCore/IdentifierMaker.hpp"
+#include <atomic>
+#include <cstddef>
+#include <list>
+#include <memory>
+
+#include <SimCore/Position.hpp>
+#include <SimCore/Messages/Track.hpp>
+#include <SimCore/SafeMap.hpp>
+#include <Entities/Tracklist/TracklistItem.hpp>
+#include <thread>
+#include <loguru.hpp>
+
+namespace TrackList 
+{
+
+    class TrackList
+    {
+        public:
+            TrackList(SimCore::Identifier OwnID);
+            ~TrackList();
+
+            void stopSanitizer();
+
+            SimCore::Identifier getTrackID(SimCore::ObjectSource source);
+
+            void addTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData);
+
+            std::shared_ptr<TracklistItem> getTrack(SimCore::Identifier TrackID);
+
+            std::vector<SimCore::Identifier> getAllIDs();
+
+            size_t size();
+
+        private:    
+
+            const SimCore::Identifier OwnID_;
+
+            SimCore::IdentifierMaker IDMaker;
+
+            void tracklistSanitizer();
+
+            void addNewTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData);
+          
+            std::map<std::string, std::shared_ptr<TracklistItem>> TrackList_;
+
+            mutable std::mutex mutex_;
+
+            std::thread sanitizerThread_;
+            std::atomic_bool sanitizerIsRunning_;
+            std::atomic_bool stopSanitizer_;
+
+
+
+    };
+
+
+}

include/Entities/Tracklist/TracklistItem.hpp

@@ -0,0 +1,100 @@
+#pragma once
+
+
+#include "SimCore/Identifier.hpp"
+#include "SimCore/Messages/Track.hpp"
+#include "SimCore/SimCore.hpp"
+#include <SimCore/Position.hpp>
+#include <chrono>
+#include <list>
+#include <memory>
+#include <vector>
+
+namespace TrackList {
+
+    struct SensorData {
+        SimCore::Identifier sensorID;
+        // SimCore::Identifier SensorTrackID;
+        std::string Sensorname;
+
+
+    };   
+
+    inline  bool operator==(const SensorData& lhs, const SensorData& rhs)
+    {
+        return lhs.sensorID == rhs.sensorID;
+
+    }
+
+
+
+    class TracklistItem
+    {
+        public: 
+            TracklistItem(std::shared_ptr<SimCore::Track> track,SensorData sensorData);
+
+             SimCore::Identifier getID();
+
+            void setPosition(SimCore::Position position);
+            SimCore::Position getPosition();
+
+            void setSpeed(double speed);
+            double getSpeed();
+
+            void setCourse(double course);
+            double getCourse();
+
+            void setPitch(double pitch);
+            double getpitch();
+
+            double getBearing();
+            double getRange();
+
+            SimCore::ObjectSource getObjectSource();
+
+            std::chrono::time_point<std::chrono::steady_clock> getLastUpdateTimestamp();
+
+            void updateTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData);
+
+            bool checkIfSensorIDIsIn(SimCore::Identifier SensorTrackID);
+
+            bool isSensorIDKnown(SimCore::Identifier);
+
+
+
+        private:
+            const SimCore::Identifier trackID_;
+
+             /// position of the track
+            SimCore::Position position_;
+            /// speed the track
+            double speed_ = 0;
+            /// course of the track
+            double course_ = 0;
+
+            double pitch_ = 0;
+
+            /// bearing  
+            double bearing_;
+            ///range in meters
+            double range_;
+            //environment (AIR,SURFACE,SUBSURFACE,SPACE)
+            SimCore::EntityKind environemnt_;
+
+            std::chrono::time_point<std::chrono::steady_clock> lastUpdateTimestamp_; 
+
+            SimCore::ObjectSource ObjectSource_;
+
+            std::vector<SensorData> SensorList;
+
+
+            bool isSensorinSensorlist(SensorData sensorData);
+
+            void addSensorDataToSensorList(SensorData sensorData);
+
+
+
+         
+    };
+
+}