ADD: added Movement calculation and a Tracklist class

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