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

197
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,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<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::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<std::thread>::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<std::chrono::milliseconds>(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<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)
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));
}
}
}