From 330bff31fb101eb0ba022bbb4085bd5844b117c7 Mon Sep 17 00:00:00 2001 From: Henry Winkel Date: Thu, 10 Aug 2023 10:47:15 +0200 Subject: [PATCH] MOD: made modification to fit to SIMCORE --- include/Entities/Entity.hpp | 8 +--- include/Entities/Sensor.hpp | 7 +--- include/Entities/Tracklist/Tracklist.hpp | 10 ++--- include/Entities/Tracklist/TracklistItem.hpp | 10 ++--- libs/SimCore | 2 +- src/Entities/Entity.cpp | 31 ++++++++++------ src/Entities/Sensor.cpp | 17 ++++----- src/Entities/Tracklist/TrackListItem.cpp | 39 +++++++------------- src/Entities/Tracklist/Tracklist.cpp | 11 +++--- 9 files changed, 62 insertions(+), 73 deletions(-) diff --git a/include/Entities/Entity.hpp b/include/Entities/Entity.hpp index b3bb54c..ff9f448 100644 --- a/include/Entities/Entity.hpp +++ b/include/Entities/Entity.hpp @@ -4,9 +4,7 @@ #include "DirectCommunicationServer.hpp" #include "Entities/Movement.hpp" -#include "SimCore/Messages/GroundThruthTrack.hpp" #include "SimCore/Messages/SimTrack.hpp" -#include "SimCore/Messages/Track.hpp" #include "SimCore/Orientation.hpp" #include "SimCore/SafeMap.hpp" #include "SimCore/SimCore.hpp" @@ -55,7 +53,6 @@ namespace Entities { Entity(const SimCore::Identifier OwnID, std::string EnttityName, WHISPER::SourceType OwnType, - SimCore::Identifier ParentID, SimCore::EntityKind EntityKind, std::uint32_t GroundTruthPort, ushort CommandPort, @@ -82,7 +79,6 @@ namespace Entities { std::shared_ptr OwnShipTrack = nullptr; std::string EntityName_; - SimCore::Identifier ParentID_; SimCore::EntityKind EntityKind_; ushort MovemntWorkerPort_; @@ -106,7 +102,7 @@ namespace Entities { void handleMovement(); - std::shared_ptr ExternalCommsServer_ = nullptr; + std::shared_ptr CommandCommsServer_ = nullptr; void handleExternalComms(std::string msg); @@ -120,7 +116,7 @@ namespace Entities { std::shared_ptr> SensorStore_; - std::shared_ptr>> Trackstore_; + std::shared_ptr>> Trackstore_; }; diff --git a/include/Entities/Sensor.hpp b/include/Entities/Sensor.hpp index 9294181..d948a9b 100644 --- a/include/Entities/Sensor.hpp +++ b/include/Entities/Sensor.hpp @@ -1,9 +1,6 @@ #pragma once - - -#include "SimCore/Messages/GroundThruthTrack.hpp" -#include "SimCore/Messages/Track.hpp" +#include "SimCore/Messages/SimTrack.hpp" #include #include @@ -25,7 +22,7 @@ namespace Entities { void stop(); protected: - std::shared_ptr>> incommingTrackMessages = nullptr; + std::shared_ptr>> incommingTrackMessages = nullptr; std::shared_ptr> incommingGroundThruthMessages = nullptr; diff --git a/include/Entities/Tracklist/Tracklist.hpp b/include/Entities/Tracklist/Tracklist.hpp index 8daacf1..366a9e8 100644 --- a/include/Entities/Tracklist/Tracklist.hpp +++ b/include/Entities/Tracklist/Tracklist.hpp @@ -3,13 +3,13 @@ #include "SimCore/Identifier.hpp" #include "SimCore/IdentifierMaker.hpp" +#include "SimCore/Messages/SimTrack.hpp" #include #include #include #include #include -#include #include #include #include @@ -28,9 +28,9 @@ namespace TrackList SimCore::Identifier getTrackID(SimCore::ObjectSource source); - void addTrack(std::shared_ptr track); + void addTrack(std::shared_ptr track); - void addTrack(std::shared_ptr track,SensorData sensorData); + void addTrack(std::shared_ptr track,SensorData sensorData); std::shared_ptr getTrack(SimCore::Identifier TrackID); @@ -46,8 +46,8 @@ namespace TrackList void tracklistSanitizer(); - void addNewTrack(std::shared_ptr track,SensorData sensorData); - void addNewTrack(std::shared_ptr track); + void addNewTrack(std::shared_ptr track,SensorData sensorData); + void addNewTrack(std::shared_ptr track); std::map> TrackList_; diff --git a/include/Entities/Tracklist/TracklistItem.hpp b/include/Entities/Tracklist/TracklistItem.hpp index 442342d..7ed277f 100644 --- a/include/Entities/Tracklist/TracklistItem.hpp +++ b/include/Entities/Tracklist/TracklistItem.hpp @@ -2,7 +2,7 @@ #include "SimCore/Identifier.hpp" -#include "SimCore/Messages/Track.hpp" +#include "SimCore/Messages/SimTrack.hpp" #include "SimCore/SimCore.hpp" #include #include @@ -31,8 +31,8 @@ namespace TrackList { class TracklistItem { public: - TracklistItem(std::shared_ptr track,SensorData sensorData); - TracklistItem(std::shared_ptr track); + TracklistItem(std::shared_ptr track,SensorData sensorData); + TracklistItem(std::shared_ptr track); SimCore::Identifier getID(); @@ -55,9 +55,9 @@ namespace TrackList { std::chrono::time_point getLastUpdateTimestamp(); - void updateTrack(std::shared_ptr track,SensorData sensorData); + void updateTrack(std::shared_ptr track,SensorData sensorData); - void updateTrack(std::shared_ptr track); + void updateTrack(std::shared_ptr track); bool checkIfSensorIDIsIn(SimCore::Identifier SensorTrackID); diff --git a/libs/SimCore b/libs/SimCore index 5e8b1b8..ed14aed 160000 --- a/libs/SimCore +++ b/libs/SimCore @@ -1 +1 @@ -Subproject commit 5e8b1b84e893946c62fab79783c13c5fe2ee3949 +Subproject commit ed14aedf0c7930ba5717de097959de1d14d851ce diff --git a/src/Entities/Entity.cpp b/src/Entities/Entity.cpp index d99330d..ccb8d6f 100644 --- a/src/Entities/Entity.cpp +++ b/src/Entities/Entity.cpp @@ -25,7 +25,7 @@ #define __MOVEMENT_SERVER_PORT__ 5556 -#define __EXTERNAL_SERVER_PORT__ 5557 +#define __SENSOR_SERVER_PORT__ 5557 @@ -35,13 +35,11 @@ namespace Entities Entity::Entity(const SimCore::Identifier OwnID, std::string EnttityName, WHISPER::SourceType OwnType, - SimCore::Identifier ParentID, SimCore::EntityKind EntityKind, std::uint32_t GroundTruthPort, ushort CommandPort, bool online): EntityName_(EnttityName), - ParentID_(ParentID), EntityKind_(EntityKind), online_(online) @@ -50,11 +48,11 @@ namespace Entities OwnShipTrack = std::make_shared(OwnID, OwnType, EntityKind); OwnShipTrack->setPosition(SimCore::Position()); - MovemtServer_ = std::make_shared(__MOVEMENT_SERVER_PORT__); + MovemtServer_ = std::make_shared(__MOVEMENT_SERVER_PORT__); - // ExternalCommsServer_ = std::make_shared(8000); + CommandCommsServer_ = std::make_shared(CommandPort); - // ExternalCommsServer_->registerMessageCallback(std::bind(&Entity::handleExternalComms,this,std::placeholders::_1)); + CommandCommsServer_->registerMessageCallback(std::bind(&Entity::handleExternalComms,this,std::placeholders::_1)); } @@ -62,6 +60,7 @@ namespace Entities { PodController_->stopAllPods(); stop(); + MovemtServer_.reset(); } @@ -187,13 +186,26 @@ namespace Entities // LOG_S(INFO)<< "new POS: LAT: "<< OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LATITUDE) << " LON: " << OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LONGITUDE); } } + + if (MovemtServer_->countClients() == 0) + { + MovementWorkerStarted = false; + } } void Entity::handleExternalComms(std::string msg) { LOG_S(INFO)<ExternalCommsServer_->countClients(); + if (MoveOrder != nullptr) + { + if(MoveOrder->Speed.isValid()) this->setSpeed(MoveOrder->Speed.getValue()); + + } + + LOG_S(INFO)<CommandCommsServer_->countClients(); + CommandCommsServer_->sendMessage("Hello Client"); } @@ -202,14 +214,11 @@ namespace Entities void Entity::MainLoop() { - auto server = DirectCommunication::DirectCommunicationServer(35000); LOG_S(INFO)<< "main loop started"; while (!stopMainLoop) { handleMovement(); - LOG_S(INFO)< +#include "SimCore/Messages/SimTrack.hpp" #include "SimCore/SimCore.hpp" #include "SimCore/UtilFunctions.hpp" #include "WHISPER/Messages/Message.hpp" @@ -24,11 +23,11 @@ namespace Entities { incommingParentMessages = std::make_shared>(); outgoingParentMessages = std::make_shared>(); - incommingTrackMessages = std::make_shared>>(); + incommingTrackMessages = std::make_shared>>(); - GroundTruthUDPService_ = std::make_shared(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,GroundTruthPort_,SimCore::UtilFunctions::implode(ip,'.'),ownIP); - ParentUDPService_ = std::make_shared(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,ParentPort,ParentIPAddress_,ownIP); + // GroundTruthUDPService_ = std::make_shared(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,GroundTruthPort_,SimCore::UtilFunctions::implode(ip,'.'),ownIP); + // ParentUDPService_ = std::make_shared(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,ParentPort,ParentIPAddress_,ownIP); }; @@ -122,7 +121,7 @@ namespace Entities { WHISPER::Message msg; incommingGroundThruthMessages->get(msg); if (msg.msgType_ == WHISPER::MsgType::GROUND_TRUTH_TRACK) { - auto GTrack = std::make_shared(std::move(SimCore::GroundTruthTrack::unpack(msg))); + auto GTrack = std::make_shared(std::move(SimCore::SimTrack::unpack(msg))); incommingTrackMessages->addElement(GTrack); } } @@ -156,9 +155,9 @@ namespace Entities { switch (msg.msgType_) { case WHISPER::MsgType::OWN_TRACK :{ - auto OwnTrack = SimCore::GroundTruthTrack::unpack(msg); + auto OwnTrack = SimCore::SimTrack::unpack(msg); // SimCore::Track OwnTrack(msg.serialize()); - auto tmpPos = OwnTrack.getPostion().getGeocentricPos(); + auto tmpPos = OwnTrack.getPosition().getGeocentricPos(); if (this->ownShipPosition_ == nullptr) { this->ownShipPosition_ = std::make_shared( tmpPos[SimCore::GeocentricPosition::X], diff --git a/src/Entities/Tracklist/TrackListItem.cpp b/src/Entities/Tracklist/TrackListItem.cpp index fc32138..8f8605d 100644 --- a/src/Entities/Tracklist/TrackListItem.cpp +++ b/src/Entities/Tracklist/TrackListItem.cpp @@ -1,4 +1,4 @@ -#include +#include "SimCore/Messages/SimTrack.hpp" #include #include @@ -9,7 +9,7 @@ namespace TrackList { - TracklistItem::TracklistItem(std::shared_ptr track,SensorData sensorData):trackID_(track->getIdentifier()) + TracklistItem::TracklistItem(std::shared_ptr track,SensorData sensorData):trackID_(track->getIdentifier()) { updateTrack(track,sensorData); @@ -17,7 +17,7 @@ namespace TrackList { } - TracklistItem::TracklistItem(std::shared_ptr track):trackID_(track->getIdentifier()) + TracklistItem::TracklistItem(std::shared_ptr track):trackID_(track->getIdentifier()) { updateTrack(track); @@ -89,22 +89,17 @@ namespace TrackList { } - void TracklistItem::updateTrack(std::shared_ptr track,SensorData sensorData) + void TracklistItem::updateTrack(std::shared_ptr track,SensorData sensorData) { - auto trackKind = track->getTrackkind(); - if (trackKind == SimCore::TrackKind::RADAR_TRACK | trackKind == SimCore::TrackKind::GROUND_TRUTH_TRACK) { - std::shared_ptr radarTrack = std::dynamic_pointer_cast(track); - - position_ = radarTrack->getPostion(); - range_ = radarTrack->getRange(); - bearing_ = radarTrack->getBearing(); - course_ = radarTrack->getCourse(); - speed_ = radarTrack->getSpeed(); + + position_ = track->getPosition(); + course_ = track->Course.getValue(); + speed_ = track->Speed.getValue(); lastUpdateTimestamp_ = std::chrono::steady_clock::now(); - } + if (isSensorinSensorlist(sensorData) != true) { addSensorDataToSensorList(sensorData); @@ -112,23 +107,15 @@ namespace TrackList { } - void TracklistItem::updateTrack(std::shared_ptr track ) + void TracklistItem::updateTrack(std::shared_ptr track ) { - auto trackKind = track->getTrackkind(); - if (trackKind == SimCore::TrackKind::RADAR_TRACK | trackKind == SimCore::TrackKind::GROUND_TRUTH_TRACK) { - std::shared_ptr radarTrack = std::dynamic_pointer_cast(track); - - position_ = radarTrack->getPostion(); - range_ = radarTrack->getRange(); - bearing_ = radarTrack->getBearing(); - course_ = radarTrack->getCourse(); - speed_ = radarTrack->getSpeed(); + position_ = track->getPosition(); + course_ = track->Course.getValue(); + speed_ = track->Speed.getValue(); lastUpdateTimestamp_ = std::chrono::steady_clock::now(); - } - } diff --git a/src/Entities/Tracklist/Tracklist.cpp b/src/Entities/Tracklist/Tracklist.cpp index 77fa140..82e1838 100644 --- a/src/Entities/Tracklist/Tracklist.cpp +++ b/src/Entities/Tracklist/Tracklist.cpp @@ -1,3 +1,4 @@ +#include "SimCore/Messages/SimTrack.hpp" #include @@ -34,11 +35,11 @@ namespace TrackList { std::lock_guard lock(mutex_); - return *IDMaker.getNewIdentifier(OwnID_.getNumber(), source).get(); + return *IDMaker.getNewIdentifier(source).get(); } - void TrackList::addTrack(std::shared_ptr track,SensorData sensorData) + void TrackList::addTrack(std::shared_ptr track,SensorData sensorData) { auto AllIDs = getAllIDs(); @@ -76,7 +77,7 @@ namespace TrackList /// @brief /// @param track - void TrackList::addTrack(std::shared_ptr track) + void TrackList::addTrack(std::shared_ptr track) { auto AllIDs = getAllIDs(); @@ -116,7 +117,7 @@ namespace TrackList // lock.unlock(); } - void TrackList::addNewTrack(std::shared_ptr track,SensorData sensorData) + void TrackList::addNewTrack(std::shared_ptr track,SensorData sensorData) { std::lock_guard lock(mutex_); @@ -126,7 +127,7 @@ namespace TrackList TrackList_.emplace(id,item); } - void TrackList::addNewTrack(std::shared_ptr track) + void TrackList::addNewTrack(std::shared_ptr track) { std::lock_guard lock(mutex_);