ADD: added Movement calculation and a Tracklist class

CMakeLists.txt

@@ -57,11 +57,21 @@ add_library(EntityLibrary STATIC
     include/Entities/Entity.hpp
     src/Entities/Entity.cpp
 
+    include/Entities/Movement.hpp
+    src/Entities/Movement.cpp
+
+
+    include/Entities/Tracklist/Tracklist.hpp
+    src/Entities/Tracklist/Tracklist.cpp
+
+    include/Entities/Tracklist/TracklistItem.hpp
+    src/Entities/Tracklist/TrackListItem.cpp
     )
 
 
 target_link_libraries(EntityLibrary
 SimCore
+eigen
 loguru
 )
 # add_dependencies(SimCore protoc)
@@ -107,12 +117,22 @@ IF (${TEST_ENTITIY_LIBRARY})
   add_executable(test_EntityClass tests/test_EntityClass.cpp)
   target_link_libraries(test_EntityClass Catch2::Catch2 EntityLibrary loguru)
   catch_discover_tests(test_EntityClass)
+  
+  add_executable(test_MovementClass tests/test_MovementClass.cpp)
+  target_link_libraries(test_MovementClass Catch2::Catch2 EntityLibrary SimCore loguru)
+  catch_discover_tests(test_MovementClass)
 
   add_executable(test_EntityImplementation tests/test_EntityImplementation.cpp)
   target_link_libraries(test_EntityImplementation EntityLibrary loguru)
 
+  add_executable(test_Tracklist tests/test_Tracklist.cpp)
+  target_link_libraries(test_Tracklist Catch2::Catch2 EntityLibrary loguru)
+  catch_discover_tests(test_Tracklist)
+
   add_executable(test_MoveOrder tests/test_MoveOrder.cpp)
   target_link_libraries(test_MoveOrder Catch2::Catch2 OrderLibrary loguru)
   catch_discover_tests(test_MoveOrder)
+
+  
   
 ENDIF()

data/movement.txt.txt

@@ -0,0 +1,171 @@
+// Topocentric Coordinates (U=east, V=north, W=up)
+struct UVW
+{
+    double dU,dV,dW;
+};
+
+// Kinematic Vector in m/s
+struct Kin_Vect
+{
+    double dVx, dVy, dVz;
+};
+
+// Geographic Coordinates in rad
+struct LLA_Rad
+{
+    double dLat, dLon, dAlt;
+};
+
+struct XYZ
+{
+    double dX = 0,dY = 0,dZ = 0;
+};
+
+// Kinematic Parameters (dCrs, dClb in rad, dSpd in m/s)
+struct Movement
+{
+    double dCrs, dSpd, dClb;
+};
+
+
+
+/** Generate_Movement
+  *
+  * Generates the kinematic data based on the
+  * geocentric kinematic vector and the loaction
+  */
+Movement Generate_Movement(Kin_Vect Kin_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord)
+{
+    Movement Result;
+
+    // Transform geocentric vector to topocentric vector
+
+
+    UVW UVW_Coord;
+
+    UVW_Coord.dU = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLon) +
+        (Kin_Dat.dVy) * cos(LLA_rCoord.dLon);
+
+    UVW_Coord.dV = -(Kin_Dat.dVx) * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) -
+        (Kin_Dat.dVy) * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
+        (Kin_Dat.dVz) * cos(LLA_rCoord.dLat);
+
+    UVW_Coord.dW = (Kin_Dat.dVx) * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
+        (Kin_Dat.dVy) * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
+        (Kin_Dat.dVz) * sin(LLA_rCoord.dLat);
+
+
+    // Transform  topocentric vector to Mov_Dat
+
+    if (abs(UVW_Coord.dU) < dEPS and abs(UVW_Coord.dV) < dEPS and abs(UVW_Coord.dW) < dEPS)
+    {
+        Result.dSpd = 0;
+
+        Result.dClb = 0;
+
+        Result.dCrs = 0;
+
+    } else
+    {
+        Result.dSpd = pow( pow(UVW_Coord.dU,2) + pow(UVW_Coord.dV,2) + pow(UVW_Coord.dW,2), 0.5);
+
+        Result.dCrs = ShiftRadCrs(atan2( UVW_Coord.dU, UVW_Coord.dV ));
+
+        Result.dClb = ShiftRadClb(atan2( UVW_Coord.dW, pow( pow(UVW_Coord.dU,2) + pow(UVW_Coord.dV,2), 0.5) ));
+    }
+
+
+    return Result;
+};
+
+
+/** Generate_Kin_Vect
+  *
+  * Generates the geocentric kinematic vector based on the
+  * kinematic data and the location
+  */
+Kin_Vect Generate_Kin_Vect(Movement Mov_Dat, LLA_Rad LLA_rCoord, XYZ XYZ_Coord)
+{
+    Kin_Vect Result;
+
+    //Transform Mov_Dat to topocentric vector
+    UVW UVW_Coord;
+    // Topocentric Coordinates (U=east, V=north, W=up)
+
+    UVW_Coord.dU = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * sin(Mov_Dat.dCrs);
+    UVW_Coord.dV = Mov_Dat.dSpd * cos(Mov_Dat.dClb) * cos(Mov_Dat.dCrs);
+    UVW_Coord.dW = Mov_Dat.dSpd * sin(Mov_Dat.dClb);
+
+    //Transform topocentric vector to geocentric vector
+
+    Result.dVx = -UVW_Coord.dU * sin(LLA_rCoord.dLon) -
+                 UVW_Coord.dV * sin(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon) +
+                 UVW_Coord.dW * cos(LLA_rCoord.dLat) * cos(LLA_rCoord.dLon);
+
+    Result.dVy = UVW_Coord.dU * cos(LLA_rCoord.dLon) -
+                 UVW_Coord.dV * sin(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon) +
+                 UVW_Coord.dW * cos(LLA_rCoord.dLat) * sin(LLA_rCoord.dLon);
+
+    Result.dVz = UVW_Coord.dV * cos(LLA_rCoord.dLat) +
+                 UVW_Coord.dW * sin(LLA_rCoord.dLat);
+
+
+    return Result;
+};
+
+
+/// Return new position after Timestep
+inline XYZ GetNewPosKin(XYZ StartPos, Kin_Vect* Motion, Acc_Vect Acceleration, unsigned int Timestep, unsigned char DR_Algorithm = 5)
+{
+    XYZ Result;
+
+    double dTimestep = static_cast<double>(Timestep);
+
+    if (DR_Algorithm == 4 or DR_Algorithm == 5)
+    {
+        Result.dX = StartPos.dX + Motion->dVx * dTimestep / 1000.0 + 0.5 * Acceleration.dAx * pow(dTimestep / 1000.0, 2);
+        Result.dY = StartPos.dY + Motion->dVy * dTimestep / 1000.0 + 0.5 * Acceleration.dAy * pow(dTimestep / 1000.0, 2);
+        Result.dZ = StartPos.dZ + Motion->dVz * dTimestep / 1000.0 + 0.5 * Acceleration.dAz * pow(dTimestep / 1000.0, 2);
+
+        Motion->dVx += Acceleration.dAx * dTimestep / 1000.0;
+        Motion->dVy += Acceleration.dAy * dTimestep / 1000.0;
+        Motion->dVz += Acceleration.dAz * dTimestep / 1000.0;
+    }
+    else
+    {
+        Result.dX = StartPos.dX + Motion->dVx * dTimestep / 1000.0;
+        Result.dY = StartPos.dY + Motion->dVy * dTimestep / 1000.0;
+        Result.dZ = StartPos.dZ + Motion->dVz * dTimestep / 1000.0;
+    };
+
+    return Result;
+}
+
+// Geographic Coordinates in rad
+struct LLA_Rad
+{
+    double dLat, dLon, dAlt;
+};
+
+
+/// Return new position after Timestep
+inline LLA_Rad GetNewPos(LLA_Rad StartPos, Movement Motion, unsigned int Timestep)
+{
+    LLA_Rad Result;
+
+    double Distance = Motion.dSpd * cos(Motion.dClb) * Timestep / 1000.0;
+
+    double DeltaAlt = Motion.dSpd * sin(Motion.dClb) * Timestep / 1000.0;
+
+    Result.dLat = asin( sin(StartPos.dLat) * cos( Distance / dR) + cos(StartPos.dLat) * sin( Distance / dR) * cos(Motion.dCrs));
+
+    Result.dLon = StartPos.dLon + atan2(sin(Motion.dCrs) * sin( Distance / dR) * cos(StartPos.dLat),
+                                        cos( Distance / dR) - sin(StartPos.dLat) * sin(Result.dLat));
+
+    Result.dAlt = StartPos.dAlt + DeltaAlt;
+
+    Result = ShiftLLA_Rad(Result);
+
+    return Result;
+}
+

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

src/Entities/Entity.cpp

@@ -1,3 +1,4 @@
+#include "Entities/Movement.hpp"
 #include "WHISPER/InternalUDPListener.hpp"
 #include "WHISPER/InternalUDPSender.hpp"
 #include "WHISPER/Messages/Message.hpp"
@@ -13,7 +14,7 @@
 
 
 
-#define calculationPeriode 100
+#define calculationPeriode 200
 
 namespace Entities
 {
@@ -33,7 +34,6 @@ namespace Entities
                    GroundTruthPort_(GroundTruthPort),
                    CommandPort_(CommandPort),
                    CommandIPAddress_(CommandIPAddress)
-
     {
     
     }
@@ -103,6 +103,10 @@ namespace Entities
 
     auto end = std::chrono::steady_clock::now();
     std::chrono::milliseconds::rep duration =  std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
+
+    Movement_.updatePosition(duration);
+
+
     specificPhysicsCalculations(duration);
     }    
 

src/Entities/Movement.cpp

@@ -0,0 +1,215 @@
+#include "GeographicLib/Constants.hpp"
+#include "SimCore/Position.hpp"
+#include "SimCore/SimCore.hpp"
+#include "SimCore/UtilFunctions.hpp"
+#include <Entities/Movement.hpp>
+#include <Eigen/Geometry>
+
+using namespace Eigen;
+
+namespace Entities 
+{
+
+    Movement::Movement()
+    {
+        course_ = 0;
+        speed_= 0;
+        pitch_ = 0;
+
+        ownOrientation_.setRoll(0);
+        ownOrientation_.setHeading(0);
+        ownOrientation_.setPitch(0);
+
+        ownPosition_.setGeodesicPos(0,0,0);
+    }
+
+
+    Movement::Movement(SimCore::Position pos):ownPosition_(pos)
+    {
+        course_ = 0;
+        speed_= 0;
+        pitch_ = 0;
+
+        ownOrientation_.setRoll(0);
+        ownOrientation_.setHeading(0);
+        ownOrientation_.setPitch(0);
+
+
+    }
+    
+    Movement::Movement(SimCore::Position pos, double course):ownPosition_(pos),course_(course)
+    {
+        speed_ = 0;
+        pitch_ = 0;
+        ownOrientation_.setRoll(0);
+        ownOrientation_.setPitch(0);
+
+
+    }
+    Movement::Movement(SimCore::Position pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed)
+    {
+        pitch_ = 0;
+        ownOrientation_.setRoll(0);
+        ownOrientation_.setPitch(0);
+
+
+    }
+    Movement::Movement(SimCore::Position pos, double course, double speed, double pitch):ownPosition_(pos),course_(course),speed_(speed),pitch_(pitch)
+    {
+        ownOrientation_.setRoll(0);
+    }
+
+
+    void  Movement::updatePosition(double dt)
+    {
+
+        if (ownPosition_.isValid()) {
+        
+            calKinematicVector();
+            
+
+            // Create ECEF coordinate system based on reference point
+            double lat = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LATITUDE));  // in radians
+            double lon = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LONGITUDE));  // in radians
+            double alt = ownPosition_.getGeodesicPos()(SimCore::HEIGHT);  // in meters
+
+            Matrix3d RotationMatrix = getRotationMatrix(lat, lon);        
+            
+            Vector3d ecefVelocityVector = RotationMatrix * kinematicVec_;
+
+
+            double X,Y,Z;    
+            X = ownPosition_.getGeocentricPos()(SimCore::X) +  ecefVelocityVector.x() * (dt / 1000.0) + 0.5 * accelerationVec_.x() * pow((dt / 1000.0), 2);
+            Y = ownPosition_.getGeocentricPos()(SimCore::Y) +  ecefVelocityVector.y() * (dt / 1000.0) + 0.5 * accelerationVec_.y() * pow((dt / 1000.0), 2);
+            Z = ownPosition_.getGeocentricPos()(SimCore::Z) +  ecefVelocityVector.z() * (dt / 1000.0) + 0.5 * accelerationVec_.z() * pow((dt / 1000.0), 2);
+
+            std::lock_guard<std::mutex> lock(mx);
+
+            kinematicVec_.x() += accelerationVec_.x() * dt / 1000.0;
+            kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
+            kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
+
+            ownPosition_.setGeocentricPos(X, Y, Z);
+            c.notify_one();
+
+        }
+
+
+    }
+
+    Eigen::Vector3d  Movement::getKinematicVector()
+    {
+
+        return kinematicVec_;
+
+    }
+
+    Eigen::Matrix3d  Movement::getRotationMatrix(double lat, double lon)
+    {
+        Matrix3d RotationMatrix;
+        RotationMatrix <<  -sin(lon),  -sin(lat)*cos(lon),  cos(lat)*cos(lon),
+                            cos(lon),  -sin(lat)*sin(lon),  cos(lat)*sin(lon),
+                                0 ,              cos(lat),               sin(lat);
+        return RotationMatrix;
+    }
+
+
+    void Movement::calKinematicVector()
+    {
+
+        Eigen::Vector3d UVW_Coord;
+
+        // Convert heading and climbing angle to radians
+        double heading_rad = SimCore::UtilFunctions::DegToRad( this->getCourse());
+        double climbing_angle_rad = SimCore::UtilFunctions::DegToRad(this->getPitch());
+        
+        // Calculate NED velocity vector components
+        double Vn = speed_ * cos(heading_rad) * cos (climbing_angle_rad) ;
+        double Ve = speed_ * sin(heading_rad) * cos (climbing_angle_rad);
+        double Vd = speed_ * sin(climbing_angle_rad);
+
+        // Create NED velocity vector using Eigen vector
+        Vector3d Vn2(Vn, Ve, -Vd);
+
+        // Create transformation matrix from NED to ENU frame using Eigen matrix
+        Matrix3d T;
+        T << 0, 1, 0,
+             1, 0, 0,
+             0, 0, -1;
+
+        // Transform NED velocity vector to ENU velocity vector using Eigen matrix multiplication
+        Vector3d Venu = T * Vn2;
+        kinematicVec_ = Venu;
+
+    }
+
+
+    SimCore::Position  Movement::getPosition()
+    {
+        std::lock_guard<std::mutex> lock(mx);
+
+        return ownPosition_;
+    }
+
+    void Movement::setPosition(double lat, double lon, double height )
+    {
+        std::lock_guard<std::mutex> lock(mx);
+
+        ownPosition_.setGeodesicPos(lat, lon, height);
+    }
+
+    void Movement::setPosition(SimCore::Position pos)
+    {
+        std::lock_guard<std::mutex> lock(mx);
+        ownPosition_ = pos;
+    }
+
+    SimCore::Orientation Movement::getownOrientation()
+    {
+        std::lock_guard<std::mutex> lock(mx);
+
+        return ownOrientation_;
+    }
+
+    Eigen::Vector3d Movement::getEulerAngles()
+    { 
+        std::lock_guard<std::mutex> lock(mx);
+        return ownOrientation_.getEulerAngles(ownPosition_.getGeodesicPos()(SimCore::LATITUDE), ownPosition_.getGeodesicPos()(SimCore::LONGITUDE));
+    }
+
+
+
+    double Movement::getCourse()
+    {
+        std::lock_guard<std::mutex> lock(mx);
+        return course_;
+    }
+    void Movement::setCourse(double course)
+    {
+        std::lock_guard<std::mutex> lock(mx);
+        ownOrientation_.setHeading(course);
+        course_ = course;
+    }
+
+    void Movement::setSpeed(double speed)
+    {
+        std::lock_guard<std::mutex> lock(mx);
+        speed_ = speed;
+    }
+
+    void Movement::setPitch(double pitch)
+    {
+        std::lock_guard<std::mutex> lock(mx);
+        ownOrientation_.setPitch(pitch);
+        pitch_ = pitch;
+    }
+
+      double Movement::getPitch() 
+      {
+        std::lock_guard<std::mutex> lock(mx);
+        return pitch_;
+      }
+
+
+
+}

src/Entities/Tracklist/TrackListItem.cpp

@@ -0,0 +1,159 @@
+#include <SimCore/Messages/RadarTrack.hpp>
+#include <SimCore/SimCore.hpp>
+#include <Entities/Tracklist/TracklistItem.hpp>
+
+#include <algorithm>
+#include <memory>
+#include <chrono>
+#include <tuple>
+
+namespace TrackList {
+
+    TracklistItem::TracklistItem(std::shared_ptr<SimCore::Track> track,SensorData sensorData):trackID_(track->getIdentifier())
+    {
+
+        updateTrack(track,sensorData);
+        addSensorDataToSensorList(sensorData);
+        
+    }
+
+      SimCore::Identifier TracklistItem::getID()
+     {
+        return trackID_;
+     }
+
+    void TracklistItem::setPosition(SimCore::Position position)
+    {
+        position_ = position;
+    }
+
+    SimCore::Position TracklistItem::getPosition()
+    {
+        return position_;
+    }
+
+    void TracklistItem::setSpeed(double speed)
+    {
+        speed_ = speed;
+    }
+
+    double TracklistItem::getSpeed()
+    {
+        return speed_;
+    }
+
+    void TracklistItem::setCourse(double course)
+    {
+        course_ = course;
+    }
+
+    double TracklistItem::getCourse()
+    {
+        return course_;;
+    }
+
+    void TracklistItem::setPitch(double pitch)
+    {
+        pitch_ = pitch;
+    }
+
+    double TracklistItem::getpitch()
+    {
+        return pitch_;
+    }
+
+    double TracklistItem::getBearing()
+    {
+        return bearing_;
+    }
+    double TracklistItem::getRange()
+    {
+        return range_;
+    }
+
+    SimCore::ObjectSource TracklistItem::getObjectSource()
+    {
+        return ObjectSource_;
+    }
+
+    std::chrono::time_point<std::chrono::steady_clock> TracklistItem::getLastUpdateTimestamp()
+    {
+        return lastUpdateTimestamp_;
+    }
+
+
+    void TracklistItem::updateTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData)
+    {   
+        
+        auto trackKind = track->getTrackkind();
+        if (trackKind == SimCore::TrackKind::RADAR_TRACK) {
+            std::shared_ptr<SimCore::RadarTrack> radarTrack = std::dynamic_pointer_cast<SimCore::RadarTrack>(track);
+
+            position_ = radarTrack->getPostion();
+            range_ = radarTrack->getRange();
+            bearing_ = radarTrack->getBearing();
+            course_ = radarTrack->getCourse();
+            speed_ = radarTrack->getSpeed();
+            lastUpdateTimestamp_ =  std::chrono::steady_clock::now();
+
+
+        }
+
+        if (isSensorinSensorlist(sensorData) != true) {
+            addSensorDataToSensorList(sensorData);
+        }
+
+    }
+
+
+    bool TracklistItem::isSensorinSensorlist(SensorData sensorData)
+    {
+        
+        auto it =  std::find(SensorList.begin(),SensorList.end(), sensorData);
+
+        if (it != SensorList.end()) {
+            return true;
+        } else {
+            return false;
+        }
+
+    }
+
+
+    bool TracklistItem::isSensorIDKnown(SimCore::Identifier SensorID)
+    {
+        for (auto i: SensorList) {
+            if (i.sensorID == SensorID)
+            {
+                return true;
+
+            }
+        }
+        return false;
+    }
+
+
+    void TracklistItem::addSensorDataToSensorList(SensorData sensorData)
+    {
+
+        if (isSensorinSensorlist(sensorData) == false) {
+
+           SensorList.emplace_back(sensorData);
+
+        } 
+    }
+
+
+
+    bool TracklistItem::checkIfSensorIDIsIn(SimCore::Identifier SensorID)
+    {
+        for (auto i:SensorList) {
+
+            if (i.sensorID == SensorID) {
+                return true;
+            }
+        }
+        return false;
+    }
+
+}

src/Entities/Tracklist/Tracklist.cpp

@@ -0,0 +1,172 @@
+
+#include <Entities/Tracklist/Tracklist.hpp>
+// #include <Entities/Tracklist/TracklistItem.hpp>
+// #include <SimCore/SimCore.hpp>
+// #include <list>
+// #include <memory>
+// #include <thread>
+
+
+#define TRACK_TIMEOUT 5 * 60 *1000
+
+namespace TrackList
+{
+
+    TrackList::TrackList(SimCore::Identifier OwnID):OwnID_(OwnID)
+    { 
+      stopSanitizer_ = false;
+      sanitizerThread_ = std::thread(&TrackList::tracklistSanitizer,this);
+    }
+
+    TrackList::~TrackList()
+    { 
+      if (sanitizerIsRunning_ == true) {
+        stopSanitizer();
+      }
+    }
+
+
+    void TrackList::stopSanitizer()
+    {
+      stopSanitizer_ = true;
+      sanitizerThread_.join();
+
+    }
+
+
+
+    SimCore::Identifier TrackList::getTrackID(SimCore::ObjectSource source)
+    {
+       std::lock_guard<std::mutex> lock(mutex_);
+
+      return  *IDMaker.getNewIdentifier(OwnID_.getNumber(), source).get();
+    }
+
+
+    void TrackList::addTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData)
+    {
+
+      auto AllIDs = getAllIDs();    
+      // std::unique_lock<std::mutex> lock(mutex_);
+      // lock.unlock();
+
+      if (AllIDs.size() == 0) {
+        addNewTrack( track,  sensorData);
+      }
+      else 
+      {
+        for (auto ID:AllIDs) {
+        // std::lock_guard<std::mutex> lock(mutex_);
+
+          auto TracklistItem = TrackList_.find(ID.serialize());
+
+          if (TracklistItem->second->isSensorIDKnown(sensorData.sensorID) == true) {
+            
+              TracklistItem->second->updateTrack(track, sensorData);
+
+          }        
+          else 
+          {
+          // lock.unlock();
+
+          addNewTrack( track,  sensorData);
+            
+
+          }
+
+        }  
+      
+      }
+    
+
+
+      // lock.unlock();
+    }
+
+    void TrackList::addNewTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData)
+    {
+      
+      std::lock_guard<std::mutex> lock(mutex_);
+      
+      auto item = std::make_shared<TracklistItem>( track, sensorData);
+      TrackList_.emplace(track->getIdentifier().serialize(),item);
+    }
+
+
+    std::shared_ptr<TracklistItem> TrackList::getTrack(SimCore::Identifier TrackID)
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+
+      std::shared_ptr<TracklistItem> result = nullptr;
+      result = TrackList_.at(TrackID.serialize());
+      if (result == nullptr) {
+        return nullptr;
+      }
+      return result;
+    }
+
+    std::vector<SimCore::Identifier> TrackList::getAllIDs()
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+
+      std::vector<SimCore::Identifier> list;
+      for (const auto& [key,value] : TrackList_) {
+        list.emplace_back(SimCore::Identifier(key));
+      }
+      
+      return list;
+    }
+
+    size_t TrackList::size()
+    {
+      std::lock_guard<std::mutex> lock(mutex_);
+      return TrackList_.size();
+    
+    }
+
+
+
+    void TrackList::tracklistSanitizer()
+    {
+      sanitizerIsRunning_ = true;
+
+      while(stopSanitizer_ == false)
+      {
+        std::unique_lock<std::mutex> lock(mutex_);
+        for ( auto it = TrackList_.begin(); it != TrackList_.end();) {
+
+          auto end = std::chrono::steady_clock::now();
+          std::chrono::milliseconds::rep duration =  std::chrono::duration_cast<std::chrono::milliseconds>(end - it->second->getLastUpdateTimestamp() ).count();
+
+          if (duration >= TRACK_TIMEOUT) {
+
+            it =  TrackList_.erase(it);
+
+          }else 
+          {
+           it++;    
+          }
+
+          lock.unlock() ;
+          std::this_thread::sleep_for(std::chrono::milliseconds(100));   
+          lock.lock();
+        }
+
+      lock.unlock();
+      lock.release();
+
+      }
+      sanitizerIsRunning_ = false;
+
+    }
+
+
+
+
+
+
+
+}
+
+
+

tests/test_EntityClass.cpp

@@ -7,6 +7,7 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <tuple>
 #define CATCH_CONFIG_MAIN
 #include <catch2/catch.hpp>
 #include <Entities/Entity.hpp>
@@ -25,21 +26,35 @@ class Ship : public Entities::Entity
         std::string CommandIPAddress):
     Entity( OwnID,EntityName,ownType,  ParentID,  EntityKind, GroundTruthPort,  CommandPort, CommandIPAddress)
           {
-            this->ownShipPosition_ = std::make_shared<SimCore::Position>();
-            this->ownShipOrientation_ = std::make_shared<SimCore::Orientation>();
+            SimCore::Position pos1;
+            pos1.setGeodesicPos(55, 6, 0);
+            Movement_.setPosition(pos1);
+            Movement_.setCourse(0);
+            Movement_.setSpeed(100);
+
+            LOG_S(INFO)<<std::endl<<Movement_.getPosition().getGeodesicPos();
+
           }
   
   private:
 
     void specificPhysicsCalculations(std::chrono::milliseconds::rep duration) override
     {
+      SimCore::Position pos1;
+      pos1.setGeodesicPos(55, 6, 0);
 
-    
-        LOG_S(INFO)<<"calculating every " << duration << "milliseconds";
+
+      LOG_S(INFO)<<std::endl<<Movement_.getPosition().getGeodesicPos();
+      double distance, bearing1;
+      std::tie(distance, bearing1) = Movement_.getPosition().distanceBearingToPosition(pos1);
+      LOG_S(INFO)<<"distance from start is:" << distance;
+      LOG_S(INFO)<<"calculating every " << duration << " milliseconds";
     };
 
     void specificReloadCharacteristicts() override 
     { 
+
+
      LOG_S(INFO)<<"loading specifications";
     };
 };
@@ -55,7 +70,7 @@ SCENARIO("Testing the SimCore Sensor")
     SimCore::Identifier ID1(0,2,false);
     Ship Ship(ID1,"FGS Hamburg",WHISPER::SourceType::SHIP,IDParent,SimCore::EntityKind::SURFACE,8000,8001,"127.0.0.1");
     Ship.start();
-    std::this_thread::sleep_for(std::chrono::milliseconds(5000));   
+    std::this_thread::sleep_for(std::chrono::milliseconds(10000));   
     Ship.stop();
     WHEN("constructing Track Object with data")
     {

tests/test_MovementClass.cpp

@@ -0,0 +1,78 @@
+
+
+#include "Entities/Movement.hpp"
+#include "SimCore/Position.hpp"
+#include "SimCore/SimCore.hpp"
+#include <memory>
+#include <string>
+#include <thread>
+#define CATCH_CONFIG_MAIN
+#include <catch2/catch.hpp>
+#include <loguru.hpp>
+
+
+
+
+
+
+SCENARIO("Testing the SimCore Sensor")
+{
+  GIVEN("different Attributes for a Movement ")
+  {
+  SimCore::Position pos1;
+    
+    WHEN("constructing Track Object with data")
+    {
+      pos1.setGeodesicPos(55, 6, 0);
+      LOG_S(INFO)<<"LAT: "<<pos1.getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< pos1.getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< pos1.getGeodesicPos()(SimCore::HEIGHT);
+
+      Entities::Movement mov(pos1);
+      mov.setCourse(90);
+      mov.setSpeed(100);//m/s
+      mov.setPitch(0);
+
+      mov.updatePosition(10000);// 10 seconds
+      
+      LOG_S(INFO)<<"LAT: "<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov.getPosition().getGeodesicPos()(SimCore::HEIGHT);
+
+      GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
+      double h2, lat2,lon2;
+      geod.Direct(55,6,90,1000,lat2,lon2);
+      SimCore::Position pos2;
+      pos2.setGeodesicPos(lat2, lon2, 0);
+      GeographicLib::GeodesicLine line = geod.InverseLine(55,6 , lat2, lon2);
+      LOG_S(INFO)<<"distance: "<<line.Distance()<< " lat2: "<< lat2 << " lon2: " << lon2;
+        
+        
+      Entities::Movement mov2(pos1);
+      mov2.setCourse(0);//degree
+      mov2.setSpeed(100);//m/s
+      mov2.setPitch(45);//degree
+
+      LOG_S(INFO)<<"LAT: "<<mov2.getPosition().getGeodesicPos()(SimCore::LATITUDE)<<" LON: "<< mov2.getPosition().getGeodesicPos()(SimCore::LONGITUDE)<< " H: "<< mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT);
+
+      double Vd = 100 * sin(45* M_PI / 180.0);
+      double h = Vd*10;
+      LOG_S(INFO)<<"höhe: "<< h;
+      double i;
+
+      for ( i = 0; i <= 10; ) {
+        mov2.updatePosition(500);
+        i += 0.5;
+      }
+
+      // LOG_S(INFO)<< mov.getEulerAngles();
+
+      THEN("check if Track attributes are correct")
+      {
+
+        // LOG_S(INFO)<<mov.getPosition().getGeodesicPos()(SimCore::LATITUDE);
+        REQUIRE((pos2.getGeocentricPos() - mov.getPosition().getGeocentricPos()).norm() <= 0.1);
+        REQUIRE((h - mov2.getPosition().getGeodesicPos()(SimCore::HEIGHT)) <= 0.1);
+
+
+
+      } //THEN
+    } // WHEN
+  } // GIVEN
+} //SCENARIO

tests/test_Tracklist.cpp

@@ -0,0 +1,87 @@
+
+#include "Entities/Tracklist/Tracklist.hpp"
+#include "Entities/Tracklist/TracklistItem.hpp"
+#include "SimCore/IdentifierMaker.hpp"
+#include "SimCore/Messages/RadarTrack.hpp"
+#include "SimCore/Position.hpp"
+#include "WHISPER/Messages/Message.hpp"
+#include <SimCore/Identifier.hpp>
+#include <SimCore/SimCore.hpp>
+#include <memory>
+#include <thread>
+#define CATCH_CONFIG_MAIN
+#include <catch2/catch.hpp>
+#include <loguru.hpp>
+
+
+
+
+
+
+SCENARIO("Testing the SimCore Sensor")
+{
+  GIVEN("different Attributes for a Track in different forms")
+  {
+   
+
+
+    SimCore::Identifier OwnID(0,1,false);
+    SimCore::IdentifierMaker IDMaker;
+    auto RadarID = IDMaker.getNewIdentifier(1,SimCore::INTERNAL);
+    auto TrackID1 = IDMaker.getNewIdentifier(1,SimCore::INTERNAL);
+    LOG_S(INFO)<<TrackID1->serialize();
+
+    TrackList::TrackList InternalTracklist(OwnID);
+    TrackList::SensorData sensor1;
+    sensor1.sensorID = *RadarID.get();
+    sensor1.Sensorname = "TRS-3D";
+
+    LOG_S(INFO)<<"making radar track";
+    auto Track1 = std::make_shared<SimCore::RadarTrack>(WHISPER::SourceType::SENSOR,*TrackID1.get());
+
+    SimCore::Position pos1 ;
+    pos1.setGeodesicPos(55, 6, 0);
+    Track1->setPosition(pos1);
+    
+    LOG_S(INFO)<<"adding radar track";
+
+    InternalTracklist.addTrack(Track1, sensor1);
+
+
+    LOG_S(INFO)<<"radar track added";
+
+
+
+
+  
+    WHEN("constructing Track Object with data")
+    {
+       
+      THEN("check if Track attributes are correct")
+      { 
+
+        REQUIRE(InternalTracklist.size() == 1);
+          REQUIRE(InternalTracklist.getTrack(Track1->getIdentifier())->getSpeed() == 0);
+
+        LOG_S(INFO)<<"add same track again";
+        
+        Track1->setSpeed(10);
+
+        InternalTracklist.addTrack(Track1, sensor1);
+        REQUIRE(InternalTracklist.getTrack(Track1->getIdentifier())->getSpeed() == 10);
+
+        REQUIRE(InternalTracklist.size() == 1);
+
+        // std::this_thread::sleep_for(std::chrono::milliseconds(2000));   
+        // REQUIRE(InternalTracklist.size() == 1);
+        // std::this_thread::sleep_for(std::chrono::milliseconds(1000));   
+        // REQUIRE(InternalTracklist.size() == 1);
+
+
+        InternalTracklist.stopSanitizer();
+
+
+      } //THEN
+    } // WHEN
+  } // GIVEN
+} //SCENARIO