diff --git a/include/SimCore/Messages/Track.hpp b/include/SimCore/Messages/Track.hpp index 6aeead9..e21767f 100644 --- a/include/SimCore/Messages/Track.hpp +++ b/include/SimCore/Messages/Track.hpp @@ -15,6 +15,7 @@ #include #include #include +#include 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); /** diff --git a/include/SimCore/Position.hpp b/include/SimCore/Position.hpp index d200aea..cefdc47 100644 --- a/include/SimCore/Position.hpp +++ b/include/SimCore/Position.hpp @@ -1,5 +1,6 @@ #pragma once +#include "SimCore/SimCore.hpp" #include #include #include @@ -7,6 +8,7 @@ #include #include #include +#include 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 earth_ = nullptr; @@ -40,6 +47,8 @@ namespace SimCore { Eigen::Vector3d GeodesicPos_; // lat long height Eigen::Vector3d GeocentricPos_; ///x y z + mutable std::mutex mx; + diff --git a/include/SimCore/Sensor.hpp b/include/SimCore/Sensor.hpp index ee98978..e215426 100644 --- a/include/SimCore/Sensor.hpp +++ b/include/SimCore/Sensor.hpp @@ -36,6 +36,8 @@ namespace SimCore { virtual void specificSensorCalculations() = 0; virtual void specificReloadCharacteristicts() = 0; + std::shared_ptr ownShipPosition_ = nullptr; + private: SimCore::Identifier OwnID_; @@ -48,8 +50,8 @@ namespace SimCore { std::shared_ptr GroundTruthUDPService_ = nullptr; std::shared_ptr ParentUDPService_ = nullptr; - void receivingData(); - void sendingData(); + void groundThruthData(); + void parentData(); void SensorCalculations(); void ReloadCharacteristicts(); @@ -69,7 +71,6 @@ namespace SimCore { std::thread sensorCalculationThread; - std::shared_ptr position_ = nullptr; }; diff --git a/src/SimCore/Messages/Track.cpp b/src/SimCore/Messages/Track.cpp index b8fe0d8..1a96060 100644 --- a/src/SimCore/Messages/Track.cpp +++ b/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() { diff --git a/src/SimCore/Position.cpp b/src/SimCore/Position.cpp index 898fa53..098e82d 100644 --- a/src/SimCore/Position.cpp +++ b/src/SimCore/Position.cpp @@ -4,14 +4,14 @@ namespace SimCore { - Position::Position() + Position::Position():mx() { earth_ = std::make_unique(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); geod_ = std::make_unique(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); wgs84_ = std::make_unique(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::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); geod_ = std::make_unique(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 lock(other.mx); + GeocentricPos_ = other.GeocentricPos_; + GeodesicPos_ = other.GeodesicPos_; + + } + Eigen::Vector3d Position::getGeocentricPos() { + std::lock_guard lock(mx); + return GeocentricPos_; } Eigen::Vector3d Position::getGeodesicPos() { + std::lock_guard lock(mx); + return GeodesicPos_; } void Position::setGeocentricPos(double X, double Y, double Z){ - GeocentricPos_(GeocentricPosition::X) = X; - GeocentricPos_(GeocentricPosition::Y) = Y; - GeocentricPos_(GeocentricPosition::Z)= Z; - - earth_->Reverse(GeocentricPos_(GeocentricPosition::X),GeocentricPos_(GeocentricPosition::Y),GeocentricPos_(GeocentricPosition::Z),GeodesicPos_(GeodesicPosition::LATITUDE),GeodesicPos_(GeodesicPosition::LONGITUDE),GeodesicPos_(GeodesicPosition::HEIGHT)); + std::lock_guard lock(mx); + + GeocentricPos_(GeocentricPosition::X) = X; + GeocentricPos_(GeocentricPosition::Y) = Y; + 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 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 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 lock(other.mx); + GeocentricPos_ = other.GeocentricPos_; + GeodesicPos_ = other.GeodesicPos_; + + return *this; + } + + } diff --git a/src/SimCore/Sensor.cpp b/src/SimCore/Sensor.cpp index 870e1b6..6e34225 100644 --- a/src/SimCore/Sensor.cpp +++ b/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 @@ -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( + 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; diff --git a/tests/test_PositionClass.cpp b/tests/test_PositionClass.cpp index 5e94c6b..8ab84c8 100644 --- a/tests/test_PositionClass.cpp +++ b/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