188 lines
4.6 KiB
C++
188 lines
4.6 KiB
C++
#include "WHISPER/InternalUDPListener.hpp"
|
|
#include "WHISPER/InternalUDPSender.hpp"
|
|
#include "WHISPER/Messages/Message.hpp"
|
|
#include "WHISPER/Messages/stringData.hpp"
|
|
#include "WHISPER/threadSafeQueue.hpp"
|
|
#include <SimCore/SimCore.hpp>
|
|
|
|
#include <Entities/Entity.hpp>
|
|
|
|
#include <chrono>
|
|
#include <memory>
|
|
#include <thread>
|
|
|
|
|
|
|
|
#define calculationPeriode 100
|
|
|
|
namespace Entities
|
|
{
|
|
|
|
Entity::Entity(const SimCore::Identifier OwnID,
|
|
std::string EnttityName,
|
|
WHISPER::SourceType OwnType,
|
|
SimCore::Identifier ParentID,
|
|
SimCore::EntityKind EntityKind,
|
|
std::uint32_t GroundTruthPort,
|
|
std::uint32_t CommandPort,
|
|
std::string CommandIPAddress):
|
|
EntityName_(EnttityName),
|
|
ownTrack_(OwnType, OwnID, SimCore::TrackKind::GROUND_TRUTH_TRACK),
|
|
ParentID_(ParentID),
|
|
EntityKind_(EntityKind),
|
|
GroundTruthPort_(GroundTruthPort),
|
|
CommandPort_(CommandPort),
|
|
CommandIPAddress_(CommandIPAddress)
|
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
Entity::~Entity()
|
|
{
|
|
stop();
|
|
}
|
|
|
|
void Entity::start()
|
|
{
|
|
|
|
stopCommandWorker = false;
|
|
stopSensorWorker = false;
|
|
stopTrackWorker = false;
|
|
stopPhysicsWorker = 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));
|
|
|
|
|
|
}
|
|
|
|
void Entity::stop()
|
|
{
|
|
stopCommandWorker = true;
|
|
stopSensorWorker = true;
|
|
stopTrackWorker = true;
|
|
stopPhysicsWorker = true;
|
|
// for (auto &th :threads)
|
|
// {
|
|
// if (th.joinable()) {
|
|
// th.join();
|
|
// }
|
|
|
|
// }
|
|
|
|
|
|
for (std::vector<std::thread>::iterator it = threads.begin(); it != threads.end();)
|
|
{
|
|
if (it->joinable())
|
|
{
|
|
it->join();
|
|
it = threads.erase(it);
|
|
}
|
|
|
|
}
|
|
|
|
LOG_S(ERROR)<< threads.size();
|
|
|
|
|
|
}
|
|
|
|
void Entity::physicsWorker()
|
|
{
|
|
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();
|
|
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)
|
|
{
|
|
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;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
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";
|
|
}
|
|
|
|
|
|
|
|
} |