ADD: made position Threadsafe, and made it possible to update the sensor position

include/SimCore/Messages/Track.hpp

@@ -15,6 +15,7 @@
 #include <memory>
 #include <string>
 #include <loguru.hpp>
+#include <type_traits>
 
 namespace SimCore {
 
@@ -54,7 +55,6 @@ namespace SimCore {
 
             /**
             * @brief constructor for building a track  which is also the trackmessage to send
-            * @param uint32_t deviceID the id of the sending device
             * @param WHISPER::SourceType sourcetype of the sending device
             * @param SimCore::Identifier object identifier
             *
@@ -62,6 +62,15 @@ namespace SimCore {
             */
             Track( WHISPER::SourceType src,SimCore::Identifier id);
 
+             /**
+            * @brief constructor for building a track  which is also the trackmessage to send
+            * @param WHISPER::SourceType sourcetype of the sending device
+            * @param SimCore::Identifier object identifier
+            * @param WHISPER::MsgType other message type than RawTrack
+            *
+            * @return 
+            */
+            Track( WHISPER::SourceType src,SimCore::Identifier id, WHISPER::MsgType type);
 
 
             /**

include/SimCore/Position.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "SimCore/SimCore.hpp"
 #include <Eigen/Core>
 #include <GeographicLib/Ellipsoid.hpp>
 #include <GeographicLib/Geocentric.hpp>
@@ -7,6 +8,7 @@
 #include <GeographicLib/GeodesicLine.hpp>
 #include <GeographicLib/Constants.hpp>
 #include <memory>
+#include <mutex>
 
 namespace SimCore {
     class Position {
@@ -14,6 +16,8 @@ namespace SimCore {
             Position();
             Position(double X, double Y, double Z);
             
+            Position(const Position& other);
+
             /**
             * @brief returns a eigen vector3d with the X, Y, Z coordinates
             * @return Eigen::Vector3d
@@ -31,6 +35,9 @@ namespace SimCore {
 
             bool operator== ( Position &lhs);
 
+            Position& operator=(const Position other); 
+           
+
 
         private:
             std::shared_ptr<GeographicLib::Geocentric> earth_ = nullptr;          
@@ -40,6 +47,8 @@ namespace SimCore {
             Eigen::Vector3d GeodesicPos_; // lat long height         
             Eigen::Vector3d GeocentricPos_; ///x y z 
 
+            mutable std::mutex mx;
+
 
 
 

include/SimCore/Sensor.hpp

@@ -36,6 +36,8 @@ namespace SimCore {
             virtual void specificSensorCalculations() = 0;
             virtual void specificReloadCharacteristicts() = 0;
 
+            std::shared_ptr<Position> ownShipPosition_ = nullptr;
+
         private:
 
             SimCore::Identifier OwnID_;
@@ -48,8 +50,8 @@ namespace SimCore {
             std::shared_ptr<WHISPER::InternalUDPService> GroundTruthUDPService_ = nullptr;    
             std::shared_ptr<WHISPER::InternalUDPService> ParentUDPService_ = nullptr;    
 
-            void receivingData();
+            void groundThruthData();
-            void sendingData();
+            void parentData();
 
             void SensorCalculations();
             void ReloadCharacteristicts();
@@ -69,7 +71,6 @@ namespace SimCore {
             
             std::thread sensorCalculationThread;
 
-            std::shared_ptr<Position> position_ = nullptr;
 
 
     };

src/SimCore/Messages/Track.cpp

@@ -15,7 +15,8 @@
 
 namespace SimCore {
 
-        Track::Track(std::string receivedMessage){
+        Track::Track(std::string receivedMessage)   
+        {
              msg = messages::header::Message();
         try {
             msg.ParseFromString(receivedMessage);
@@ -30,13 +31,10 @@ namespace SimCore {
                 }     
             }
 
-            // messages::track::Identifier ID = trackMessage.entityidentifier();
             ID_ = SimCore::Identifier(trackMessage.mutable_entityidentifier()->parent(),
                                              trackMessage.mutable_entityidentifier()->number(),
                                              (SimCore::ObjectSource)trackMessage.mutable_entityidentifier()->external());
             external_ = trackMessage.mutable_entityidentifier()->external();
-            // trackNo_ = trackMessage.trackno();
-            // external_ = trackMessage.external();
             speed_ = trackMessage.speed();
             course_ = trackMessage.course();
 
@@ -59,6 +57,13 @@ namespace SimCore {
              packToMessage();
 
         }
+        
+        Track::Track( WHISPER::SourceType src,SimCore::Identifier id, WHISPER::MsgType type):
+        Message(id.getParentNumber(),id.getNumber(),WHISPER::MsgTopics::TRACK,type,src),external_(id.isExternal()),ID_(id)
+        {
+            packToMessage();
+        }
+
 
         void Track::packToMessage()
         {

src/SimCore/Position.cpp

@@ -4,14 +4,14 @@
 
 namespace SimCore {
 
-    Position::Position()
+    Position::Position():mx()
     {
         earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
         geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
         wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
         
     }
-    Position::Position(double X, double Y, double Z)
+    Position::Position(double X, double Y, double Z):mx()
     {
         earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
         geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
@@ -19,26 +19,42 @@ namespace SimCore {
         setGeocentricPos( X, Y,  Z);
     }
 
+
+    Position::Position(const Position& other){
+                    std::lock_guard<std::mutex> lock(other.mx);
+                    GeocentricPos_ = other.GeocentricPos_;
+                    GeodesicPos_ = other.GeodesicPos_;
+        
+    }
+
     Eigen::Vector3d Position::getGeocentricPos()
     {
+        std::lock_guard<std::mutex> lock(mx);
+
         return GeocentricPos_;
 
     }
 
     Eigen::Vector3d Position::getGeodesicPos()
     {
+        std::lock_guard<std::mutex> lock(mx);
+
         return GeodesicPos_;
     }
 
     void Position::setGeocentricPos(double X, double Y, double Z){
-    GeocentricPos_(GeocentricPosition::X) = X; 
+        std::lock_guard<std::mutex> lock(mx);
-    GeocentricPos_(GeocentricPosition::Y) = Y;     
+
-    GeocentricPos_(GeocentricPosition::Z)= Z;     
+        GeocentricPos_(GeocentricPosition::X) = X; 
-    
+        GeocentricPos_(GeocentricPosition::Y) = Y;     
-    earth_->Reverse(GeocentricPos_(GeocentricPosition::X),GeocentricPos_(GeocentricPosition::Y),GeocentricPos_(GeocentricPosition::Z),GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT));
+        GeocentricPos_(GeocentricPosition::Z)= Z;     
+        
+        earth_->Reverse(GeocentricPos_(GeocentricPosition::X),GeocentricPos_(GeocentricPosition::Y),GeocentricPos_(GeocentricPosition::Z),GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT));
     }   
 
     void Position::setGeodesicPos(double lat, double lon, int height){
+        std::lock_guard<std::mutex> lock(mx);
+
         GeodesicPos_(GeodesicPosition::LATITUDE) = lat;
         GeodesicPos_(GeodesicPosition::LONGITUDE) = lon;
         GeodesicPos_(GeodesicPosition::HEIGHT) = height;
@@ -47,6 +63,8 @@ namespace SimCore {
 
     bool Position::operator==( Position &lhs)
     {
+        std::lock_guard<std::mutex> lock(mx);
+
         if (this->GeocentricPos_ == lhs.getGeocentricPos()) {
             return true;
         }else{
@@ -55,6 +73,16 @@ namespace SimCore {
 
     }
 
+    Position& Position::operator=(const Position other)
+    {
+        std::lock_guard<std::mutex> lock(other.mx);
+        GeocentricPos_ = other.GeocentricPos_;
+        GeodesicPos_ = other.GeodesicPos_;
+        
+        return *this;
+    }
+
+
 
 }
 

src/SimCore/Sensor.cpp

@@ -1,4 +1,6 @@
 #include "SimCore/Messages/Track.hpp"
+#include "SimCore/Position.hpp"
+#include "SimCore/SimCore.hpp"
 #include "SimCore/UtilFunctions.hpp"
 #include "WHISPER/Messages/Message.hpp"
 #include <SimCore/Sensor.hpp>
@@ -66,10 +68,10 @@ namespace SimCore {
 
 
     stopReceivingGroundThruth = false;
-    receiveGroundTruthThread  = std::thread(&Sensor::receivingData,this);
+    receiveGroundTruthThread  = std::thread(&Sensor::groundThruthData,this);
 
     stopsendCalculatedData = false;
-    sendCalculatedDataThread = std::thread(&Sensor::sendingData,this);
+    sendCalculatedDataThread = std::thread(&Sensor::parentData,this);
 
     stopCalculationData = false;
     sensorCalculationThread = std::thread(&Sensor::SensorCalculations,this);
@@ -107,7 +109,7 @@ namespace SimCore {
     }
 
 
-    void Sensor::receivingData()
+    void Sensor::groundThruthData()
     {   
         this->ReceivingGroundThruthIsRunnung = true;
         GroundTruthUDPService_->connect(incommingGroundThruthMessages);
@@ -134,7 +136,7 @@ namespace SimCore {
         this->ReceivingGroundThruthIsRunnung = false;
     }
 
-    void Sensor::sendingData()
+    void Sensor::parentData()
     {
         this->sendCalculatedDataIsRunnung = true;
 
@@ -145,9 +147,31 @@ namespace SimCore {
             
             if (incommingParentMessages->size() > 0) {
                  WHISPER::Message msg; 
-                incommingGroundThruthMessages->get(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) {
+                        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;

tests/test_PositionClass.cpp

@@ -24,9 +24,11 @@ SCENARIO("Testing the SimCorePositionClass")
     WHEN("constructing Position Object with data")
     {
     SimCore::Position pos1(  GeocentPos1(SimCore::GeocentricPosition::X),  GeocentPos1(SimCore::GeocentricPosition::Y),  GeocentPos1(SimCore::GeocentricPosition::Z));
-    SimCore::Position pos1b;
+    SimCore::Position pos1b, pos2;
     pos1b.setGeodesicPos(GeodesPos1(SimCore::GeodesicPosition::LATITUDE), GeodesPos1(SimCore::GeodesicPosition::LONGITUDE), GeodesPos1(SimCore::GeodesicPosition::HEIGHT)); 
-
+    
+     pos2 = pos1;
+    
       THEN("positions attributes are correct")
       {
         REQUIRE(pos1.getGeocentricPos()(SimCore::GeocentricPosition::X) == GeocentPos1(SimCore::GeocentricPosition::X));
@@ -38,6 +40,11 @@ SCENARIO("Testing the SimCorePositionClass")
         REQUIRE(std::abs(pos1b.getGeocentricPos()(SimCore::Y) - GeocentPos1(SimCore::Y)) <= 0.001 );
         REQUIRE(std::abs(pos1b.getGeocentricPos()(SimCore::Z) - GeocentPos1(SimCore::Z)) <= 0.001);
 
+
+        REQUIRE(pos2.getGeocentricPos() == pos1.getGeocentricPos());
+        REQUIRE(pos2.getGeodesicPos() == pos1.getGeodesicPos());
+
+
       } //THEN
     } // WHEN
   } // GIVEN