henry/EntityLibrary
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

ADD: added new version of entity base class

Browse Source
This commit is contained in:
Henry Winkel
2023-07-05 17:58:59 +02:00
parent bbbd5b96df
commit 64f37e59f5
3 changed files with 103 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
include/Entities/Entity.hpp
View File

@@ -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<WHISPER::threadSafeQueue<WHISPER::Message>> incommingCommandMessages = nullptr;
std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> outgoingCommandMessages = nullptr;
virtual void specificPhysicsCalculations(std::chrono::milliseconds::rep duration) = 0;
virtual void specificReloadCharacteristicts() = 0;
std::unique_ptr<Entities::Movement> Movement_ = nullptr;
protected:
std::shared_ptr<SimCore::SimTrack> 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<std::thread> threads;
std::atomic<bool> stopCommandWorker = false;
std::atomic<bool> stopSensorWorker = false;
std::atomic<bool> stopTrackWorker = false;
std::atomic<bool> stopPhysicsWorker = false;
std::atomic<bool> stopMainLoop = false;
void MainLoop();
void startMovementWorker();
std::atomic<bool> MovementWorkerStarted = false;
std::shared_ptr<DirectCommunication::DirectCommunicationServer> MovemtServer_ = nullptr;
std::atomic<bool> physicsIsRunning = false;
void CommandWorker();
void SensorWorker();
void TrackWorker();
void physicsWorker();
void startSensor();
std::shared_ptr<WHISPER::InternalUDPSender> GroundTruthUDPSender_ = nullptr;
std::shared_ptr<std::list<Entities::SensorClientData>> SensorStore_;

2
libs/SimCore

Submodule libs/SimCore updated: d4c12f3b09...1a878af6a5

163
src/Entities/Entity.cpp
View File

@@ -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,16 +27,17 @@ 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<SimCore::SimTrack>(OwnID, OwnType, EntityKind);
MovemtServer_ = std::make_shared<DirectCommunication::DirectCommunicationServer>(__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::MainLoop,this));
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));
}
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<std::thread>::iterator it = threads.begin(); it != threads.end();)
@@ -90,109 +100,48 @@ 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<std::chrono::milliseconds>(end - start).count();
if(Movement_ != nullptr)
LOG_S(INFO)<< "TODO: starting the movement app from here";
if (MovemtServer_->countClients() > 0 )
{
// Movement_->updatePosition(duration);
Movement_->updatePositionSimple(duration);
}
// LOG_S(INFO)<<"Entity class working";
specificPhysicsCalculations(duration);
}
physicsIsRunning = false;
}
void Entity::CommandWorker()
auto msg = MovemtServer_->getLatestMessage();
if (*msg.get() == "Hello Server")
{
auto CommandUDPListener = std::make_shared<WHISPER::InternalUDPListener>(CommandPort_) ;
auto receiverQueue = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
CommandUDPListener->connect(receiverQueue);
auto CommandUDPSender = std::make_shared<WHISPER::InternalUDPSender>(CommandIPAddress_,CommandPort_);
while (!stopCommandWorker)
{
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;
MovemtServer_->sendMessage(OwnShipTrack->buildMessage().serialize());
}
}
}
// LOG_S(INFO)<<"hello from command worker";
std::this_thread::sleep_for(std::chrono::milliseconds(900));
}
}
void Entity::SensorWorker()
void Entity::MainLoop()
{
while (!stopSensorWorker)
while (!stopMainLoop)
{
// LOG_S(INFO)<<"hello from sensor worker";
std::this_thread::sleep_for(std::chrono::milliseconds(900));
}
}
void Entity::TrackWorker()
if (!MovementWorkerStarted)
{
while (!stopTrackWorker)
{
// LOG_S(INFO)<<"hello from track worker";
std::this_thread::sleep_for(std::chrono::milliseconds(900));
startMovementWorker();
}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
}
void Entity::startSensor()
{
LOG_S(ERROR)<< "starting new pods is not implemented yet";
}