From 64f37e59f570ba89a7bb0070afebbda68a11d5f4 Mon Sep 17 00:00:00 2001 From: Henry Winkel Date: Wed, 5 Jul 2023 17:58:59 +0200 Subject: [PATCH] ADD: added new version of entity base class --- include/Entities/Entity.hpp | 57 ++++++----- libs/SimCore | 2 +- src/Entities/Entity.cpp | 197 +++++++++++++----------------------- 3 files changed, 103 insertions(+), 153 deletions(-) diff --git a/include/Entities/Entity.hpp b/include/Entities/Entity.hpp index 07c5bfe..73ea315 100644 --- a/include/Entities/Entity.hpp +++ b/include/Entities/Entity.hpp @@ -2,8 +2,10 @@ +#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" @@ -53,62 +55,61 @@ namespace Entities { SimCore::Identifier ParentID, SimCore::EntityKind EntityKind, std::uint32_t GroundTruthPort, - std::uint32_t CommandPort, - std::string CommandIPAddress); + ushort CommandPort); ~Entity(); void start(); void stop(); + void setPosition(SimCore::Position); + void setSpeed(double speed); + void setCourse(double course); + void setPitch( double pitch); protected: - std::shared_ptr> incommingCommandMessages = nullptr; - std::shared_ptr> outgoingCommandMessages = nullptr; + virtual void specificPhysicsCalculations(std::chrono::milliseconds::rep duration) = 0; virtual void specificReloadCharacteristicts() = 0; - std::unique_ptr Movement_ = nullptr; protected: + std::shared_ptr OwnShipTrack = nullptr; + std::string EntityName_; - SimCore::GroundTruthTrack ownTrack_; SimCore::Identifier ParentID_; SimCore::EntityKind EntityKind_; - std::uint32_t GroundTruthPort_; - std::uint32_t CommandPort_; - std::string CommandIPAddress_; + + ushort MovemntWorkerPort_; + + + + private: - - - - std::vector threads; + std::atomic stopMainLoop = false; + void MainLoop(); + + + void startMovementWorker(); + std::atomic MovementWorkerStarted = false; + std::shared_ptr MovemtServer_ = nullptr; + + + + + + - std::atomic stopCommandWorker = false; - std::atomic stopSensorWorker = false; - std::atomic stopTrackWorker = false; - std::atomic stopPhysicsWorker = false; - - - std::atomic physicsIsRunning = false; - - - void CommandWorker(); - void SensorWorker(); - void TrackWorker(); - void physicsWorker(); - void startSensor(); - std::shared_ptr GroundTruthUDPSender_ = nullptr; std::shared_ptr> SensorStore_; diff --git a/libs/SimCore b/libs/SimCore index d4c12f3..1a878af 160000 --- a/libs/SimCore +++ b/libs/SimCore @@ -1 +1 @@ -Subproject commit d4c12f3b092f30d6218f0c0a4c2eec2f619aaf96 +Subproject commit 1a878af6a57ca7d94bcf7bcd9d688340e6ab6c05 diff --git a/src/Entities/Entity.cpp b/src/Entities/Entity.cpp index 1207c93..f94e37f 100644 --- a/src/Entities/Entity.cpp +++ b/src/Entities/Entity.cpp @@ -1,4 +1,6 @@ +#include "DirectCommunicationServer.hpp" #include "Entities/Movement.hpp" +#include "SimCore/Messages/SimTrack.hpp" #include "WHISPER/InternalUDPListener.hpp" #include "WHISPER/InternalUDPSender.hpp" #include "WHISPER/Messages/Message.hpp" @@ -14,7 +16,7 @@ -#define calculationPeriode 200 +#define __MOVEMENT_SERVER_PORT__ 5556 namespace Entities { @@ -25,17 +27,18 @@ namespace Entities SimCore::Identifier ParentID, SimCore::EntityKind EntityKind, std::uint32_t GroundTruthPort, - std::uint32_t CommandPort, - std::string CommandIPAddress): + ushort CommandPort): EntityName_(EnttityName), - ownTrack_(OwnType, OwnID, SimCore::TrackKind::GROUND_TRUTH_TRACK), ParentID_(ParentID), - EntityKind_(EntityKind), - GroundTruthPort_(GroundTruthPort), - CommandPort_(CommandPort), - CommandIPAddress_(CommandIPAddress) + EntityKind_(EntityKind) + { - + OwnShipTrack = std::make_shared(OwnID, OwnType, EntityKind); + + MovemtServer_ = std::make_shared(__MOVEMENT_SERVER_PORT__); + + + } @@ -44,35 +47,42 @@ namespace Entities stop(); } + + + void Entity::setPosition(SimCore::Position pos) + { + OwnShipTrack->setPosition(pos); + } + void Entity::setSpeed(double val) + { + OwnShipTrack->Speed.setValue(val); + } + void Entity::setCourse(double val) + { + OwnShipTrack->Course.setValue(val); + + } + void Entity::setPitch( double val) + { + OwnShipTrack->Pitch.setValue(val); + } + void Entity::start() { - stopCommandWorker = false; - stopSensorWorker = false; - stopTrackWorker = false; - stopPhysicsWorker = false; + stopMainLoop = false; + - threads.emplace_back(std::thread(&Entity::CommandWorker,this)); - threads.emplace_back(std::thread(&Entity::SensorWorker,this)); - threads.emplace_back(std::thread(&Entity::TrackWorker,this)); - threads.emplace_back(std::thread(&Entity::physicsWorker,this)); + threads.emplace_back(std::thread(&Entity::MainLoop,this)); + } void Entity::stop() { - stopCommandWorker = true; - stopSensorWorker = true; - stopTrackWorker = true; - stopPhysicsWorker = true; - // for (auto &th :threads) - // { - // if (th.joinable()) { - // th.join(); - // } - - // } + stopMainLoop = true; + for (std::vector::iterator it = threads.begin(); it != threads.end();) @@ -90,110 +100,49 @@ namespace Entities } - void Entity::physicsWorker() + void Entity::startMovementWorker() { - physicsIsRunning = true; - while (!stopPhysicsWorker) { - auto start = std::chrono::steady_clock::now(); - - - std::this_thread::sleep_for(std::chrono::milliseconds(calculationPeriode)); - - - auto end = std::chrono::steady_clock::now(); - std::chrono::milliseconds::rep duration = std::chrono::duration_cast(end - start).count(); - - - if(Movement_ != nullptr) - { - // Movement_->updatePosition(duration); - Movement_->updatePositionSimple(duration); - - } - // LOG_S(INFO)<<"Entity class working"; - - - specificPhysicsCalculations(duration); - } - - physicsIsRunning = false; - } - - - - void Entity::CommandWorker() - { - auto CommandUDPListener = std::make_shared(CommandPort_) ; - auto receiverQueue = std::make_shared>(); - CommandUDPListener->connect(receiverQueue); - - - auto CommandUDPSender = std::make_shared(CommandIPAddress_,CommandPort_); - - - while (!stopCommandWorker) + LOG_S(INFO)<< "TODO: starting the movement app from here"; + if (MovemtServer_->countClients() > 0 ) { - if (receiverQueue->size() > 0) { - - auto msg = WHISPER::Message(); - receiverQueue->get(msg); - - switch (msg.msgType_) { - - case WHISPER::MsgType::STRINGDATA :{ - WHISPER::StringData stringMsg = WHISPER::StringData(msg.serialize()); - std::string str = stringMsg.data_; - break; - } - - case WHISPER::MsgType::COMMAND: { - WHISPER::StringData string = WHISPER::StringData(msg.serialize()); - break; - } - } - + auto msg = MovemtServer_->getLatestMessage(); + if (*msg.get() == "Hello Server") + { + MovemtServer_->sendMessage(OwnShipTrack->buildMessage().serialize()); } - - - // LOG_S(INFO)<<"hello from command worker"; - std::this_thread::sleep_for(std::chrono::milliseconds(900)); - } - - - } - - void Entity::SensorWorker() - { - - - while (!stopSensorWorker) - { - // LOG_S(INFO)<<"hello from sensor worker"; - std::this_thread::sleep_for(std::chrono::milliseconds(900)); - - } - - } - - void Entity::TrackWorker() - { - while (!stopTrackWorker) - { - // LOG_S(INFO)<<"hello from track worker"; - std::this_thread::sleep_for(std::chrono::milliseconds(900)); - - } - - } - - void Entity::startSensor() - { - LOG_S(ERROR)<< "starting new pods is not implemented yet"; + + } + void Entity::MainLoop() + { + + + while (!stopMainLoop) + { + if (!MovementWorkerStarted) + { + startMovementWorker(); + } + + + + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + } + + + } + + + + + + + } \ No newline at end of file