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

ADD: updated Sensor class

Browse Source
This commit is contained in:
Henry Winkel
2023-11-08 15:04:58 +01:00
parent 44f5ce09de
commit ab32c5e8df
3 changed files with 190 additions and 205 deletions
Download Patch File Download Diff File Expand all files Collapse all files

68
include/Entities/Sensor.hpp
View File

@@ -1,6 +1,8 @@
#pragma once
#include "DirectCommunicationClient.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "WHISPER/InternalUDPListener.hpp"
#include <WHISPER/InternalUDPService.hpp>
#include <SimCore/Identifier.hpp>
@@ -9,66 +11,68 @@
#include <SimCore/SimCore.hpp>
#include <SimCore/Identifier.hpp>
#include <SimCore/Position.hpp>
#include <atomic>
#include <memory>
#include <string>
#include <thread>
namespace Entities {
class Sensor {
public:
Sensor(SimCore::Identifier OwnID, SimCore::Identifier ParentID, SimCore::SensorKinds SensorKind,std::uint32_t GroundTruthPort, std::uint32_t ParentPort,std::string ParentIPAddress);
~Sensor();
Sensor(
SimCore::Identifier OwnID,
SimCore::Identifier OwnShipID,
SimCore::SensorKinds SensorKind,
std::string GroundTruthAddress,
std::uint32_t GroundTruthPort,
std::uint32_t ParentPort,
std::string ParentIPAddress);
void start();
void stop();
protected:
std::shared_ptr<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>> incommingTrackMessages = nullptr;
std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> incommingGroundThruthMessages = nullptr;
std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> outgoingGroundThruthMessages = nullptr;
std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> incommingParentMessages = nullptr;
std::shared_ptr<WHISPER::threadSafeQueue<WHISPER::Message>> outgoingParentMessages = nullptr;
virtual void specificSensorCalculations(std::unique_ptr<SimCore::SimTrack> track) = 0;
virtual void specificSensorCalculations() = 0;
virtual void specificReloadCharacteristicts() = 0;
std::shared_ptr<SimCore::SimTrack> OwnShipTrack_ = nullptr;
std::shared_ptr<SimCore::Position> ownShipPosition_ = nullptr;
SimCore::Identifier OwnID_;
const SimCore::Identifier OwnID_;
const SimCore::Identifier OwnShipID;
SimCore::SensorKinds SensorKind_;
std::shared_ptr<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>> recognisedTracks_ = nullptr;
private:
SimCore::Identifier ParentID_;
SimCore::SensorKinds SensorKind_;
std::uint32_t GroundTruthPort_;
std::string GroundTruthAddr_;
std::uint32_t ParentPort_;
std::string ParentIPAddress_;
std::shared_ptr<WHISPER::InternalUDPService> GroundTruthUDPService_ = nullptr;
std::shared_ptr<WHISPER::InternalUDPService> ParentUDPService_ = nullptr;
std::unique_ptr<WHISPER::InternalUDPListener> GroundTruthUDPListener_;
void groundThruthData();
void parentData();
void SensorCalculations();
void ReloadCharacteristicts();
std::atomic<bool> stopReceivingGroundThruth = false;
std::atomic<bool> ReceivingGroundThruthIsRunnung = false;
std::atomic<bool> stopsendCalculatedData = false;
std::atomic<bool> sendCalculatedDataIsRunnung = false;
std::atomic<bool> stopCalculationData = false;
std::atomic<bool> CalculationIsRunnung = false;
std::unique_ptr<DirectCommunication::DirectCommunicationClient> client_;
std::thread receiveGroundTruthThread;
std::thread sendCalculatedDataThread;
std::thread UpdateOwnShipThread_;
std::thread sensorCalculationThread;
std::atomic_bool stopOwnShipUpdater_ = false;
void handleGroundThruthMessage(std::string msg);
void updateOwnShipFunction();
void handlServerMessages(std::string msg);
void HandleOrders(WHISPER::Message WHmsg);
void setupOwnShip(std::shared_ptr<SimCore::SimTrack> ownShipTracK);
};

2
libs/SimCore

Submodule libs/SimCore updated: 49dd5e295f...6845716395

305
src/Entities/Sensor.cpp
View File

@@ -1,30 +1,49 @@
#include <SimCore/Position.hpp>
#include "DirectCommunicationClient.hpp"
#include "Orders/Order.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/SimCore.hpp"
#include "SimCore/UtilFunctions.hpp"
#include "WHISPER/InternalUDPListener.hpp"
#include "WHISPER/Messages/Message.hpp"
#include <Entities/Sensor.hpp>
#include <memory>
#include <thread>
#include <utility>
namespace Entities {
Sensor::Sensor(SimCore::Identifier OwnID, SimCore::Identifier ParentID, SimCore::SensorKinds SensorKind,std::uint32_t GroundTruthPort, std::uint32_t ParentPort,std::string ParentIPAddress):
OwnID_(OwnID),ParentID_(ParentID),SensorKind_(SensorKind),GroundTruthPort_(GroundTruthPort),ParentPort_(ParentPort),ParentIPAddress_(ParentIPAddress)
Sensor::Sensor(
SimCore::Identifier OwnID,
SimCore::Identifier OwnShipID,
SimCore::SensorKinds SensorKind,
std::string GroundTruthAddress,
std::uint32_t GroundTruthPort,
std::uint32_t ParentPort,
std::string ParentIPAddress):
OwnID_(OwnID),
OwnShipID(OwnShipID),
SensorKind_(SensorKind),
GroundTruthAddr_(GroundTruthAddress),
GroundTruthPort_(GroundTruthPort),
ParentPort_(ParentPort),
ParentIPAddress_(ParentIPAddress)
{
std::string ownIP = SimCore::UtilFunctions::getOwnIP();
auto ip = SimCore::UtilFunctions::explode(ownIP, '.');
ip[3] = "255";
LOG_S(INFO)<<SimCore::UtilFunctions::implode(ip,'.');
incommingGroundThruthMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
outgoingGroundThruthMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
incommingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
outgoingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
recognisedTracks_ = std::make_shared<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>>();
incommingTrackMessages = std::make_shared<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>>();
GroundTruthUDPListener_ = std::make_unique<WHISPER::InternalUDPListener>(GroundTruthAddr_,GroundTruthPort_);
GroundTruthUDPListener_->registerMessageCallback(std::bind(&Sensor::handleGroundThruthMessage,this,std::placeholders::_1));
GroundTruthUDPListener_->connect();
GroundTruthUDPListener_->subscribe(WHISPER::MsgTopics::TRACK);
client_ = std::make_unique<DirectCommunication::DirectCommunicationClient>(ParentPort_,ParentIPAddress_);
client_->registerMessageCallback(std::bind(&Sensor::handlServerMessages,this,std::placeholders::_1));
client_->sendMessage("Hello Server");
// GroundTruthUDPService_ = std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,GroundTruthPort_,SimCore::UtilFunctions::implode(ip,'.'),ownIP);
// ParentUDPService_ = std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,ParentPort,ParentIPAddress_,ownIP);
@@ -33,196 +52,158 @@ namespace Entities {
};
Sensor::~Sensor(){
// this->stop();
if (ReceivingGroundThruthIsRunnung && sendCalculatedDataIsRunnung && CalculationIsRunnung) {
this->stop();
}
this->incommingGroundThruthMessages.reset();
this->outgoingGroundThruthMessages.reset();
// GroundTruthUDPService_->disconnect();
while (!this->GroundTruthUDPService_.unique()) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
this->GroundTruthUDPService_.reset();
LOG_S(INFO)<<"groundThruth is closed";
ParentUDPService_->disconnect();
while (!this->ParentUDPService_.unique()) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
this->ParentUDPService_.reset();
LOG_S(INFO)<<"all destructed";
}
void Sensor::start(){
ReloadCharacteristicts();
stopReceivingGroundThruth = false;
receiveGroundTruthThread = std::thread(&Sensor::groundThruthData,this);
stopsendCalculatedData = false;
sendCalculatedDataThread = std::thread(&Sensor::parentData,this);
this->stopOwnShipUpdater_ = false;
UpdateOwnShipThread_ = std::thread(&Sensor::updateOwnShipFunction,this);
stopCalculationData = false;
sensorCalculationThread = std::thread(&Sensor::SensorCalculations,this);
}
void Sensor::stop() {
this->stopOwnShipUpdater_ = true;
UpdateOwnShipThread_.join();
while (ReceivingGroundThruthIsRunnung == true ) {
stopReceivingGroundThruth = true;
LOG_S(INFO)<<"waiting for groundthruth thread thread";
if (receiveGroundTruthThread.joinable() == true ) {
receiveGroundTruthThread.join();
}
}
GroundTruthUDPListener_->stop();
GroundTruthUDPListener_.reset();
while (sendCalculatedDataIsRunnung == true ) {
stopsendCalculatedData = true;
LOG_S(INFO)<<"waiting for parent sending thread thread";
if (sendCalculatedDataThread.joinable() == true ) {
sendCalculatedDataThread.join();
}
}
while (CalculationIsRunnung == true ) {
stopCalculationData = true;
LOG_S(INFO)<<"waiting for calculation thread thread";
if (sensorCalculationThread.joinable() == true ) {
client_->disconnect();
client_.reset();
sensorCalculationThread.join();
}
}
LOG_S(INFO)<<"all stopped";
}
void Sensor::groundThruthData()
void Sensor::handlServerMessages(std::string msg)
{
try {
WHISPER::Message whisperMsg(msg);
// LOG_S(INFO)<<"New Message from TCP Client";
// LOG_S(INFO)<<"Message Type is: " << whisperMsg.msgType_;
switch (whisperMsg.msgType_)
{
this->ReceivingGroundThruthIsRunnung = true;
GroundTruthUDPService_->connect(incommingGroundThruthMessages);
GroundTruthUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::TRACK]);
while (stopReceivingGroundThruth == false) {
if (incommingGroundThruthMessages->size() > 0) {
WHISPER::Message msg;
incommingGroundThruthMessages->get(msg);
if (msg.msgType_ == WHISPER::MsgType::GROUND_TRUTH_TRACK) {
auto GTrack = std::make_shared<SimCore::SimTrack>(std::move(SimCore::SimTrack::unpack(msg)));
incommingTrackMessages->addElement(GTrack);
}
}
if (outgoingGroundThruthMessages->size() > 0) {
WHISPER::Message msg;
outgoingGroundThruthMessages->get(msg);
GroundTruthUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]);
}
}
GroundTruthUDPService_->disconnect();
this->ReceivingGroundThruthIsRunnung = false;
}
void Sensor::parentData()
case WHISPER::MsgType::ORDER:
{
this->sendCalculatedDataIsRunnung = true;
ParentUDPService_->connect(incommingParentMessages);
ParentUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::COMMANDS]);
LOG_S(INFO)<<"ORDER";
HandleOrders(whisperMsg);
while (stopsendCalculatedData == false) {
if (incommingParentMessages->size() > 0) {
WHISPER::Message msg;
incommingParentMessages->get(msg);
LOG_S(INFO)<< "Message received from Parent is" << msg.msgType_;
std::uint32_t type = 0;
break;
}
case WHISPER::MsgType::SIM_TRACK:
{
switch (msg.msgType_) {
case WHISPER::MsgType::OWN_TRACK :{
OwnShipTrack_ = SimCore::SimTrack::unpack(msg);
if (OwnShipTrack_ != nullptr)
{
// LOG_S(INFO)<<"own SHip data received";
setupOwnShip(OwnShipTrack_);
auto OwnTrack = SimCore::SimTrack::unpack(msg);
// SimCore::Track OwnTrack(msg.serialize());
auto tmpPos = OwnTrack.getPosition().getGeocentricPos();
if (this->ownShipPosition_ == nullptr) {
this->ownShipPosition_ = std::make_shared<SimCore::Position>(
tmpPos[SimCore::GeocentricPosition::X],
tmpPos[SimCore::GeocentricPosition::Y],
tmpPos[SimCore::GeocentricPosition::Z]);
}else {
this->ownShipPosition_->setGeocentricPos(
tmpPos[SimCore::GeocentricPosition::X],
tmpPos[SimCore::GeocentricPosition::Y],
tmpPos[SimCore::GeocentricPosition::Z]);
}
break;
}
case WHISPER::MsgType::COMMAND: {
}
} catch (std::exception &e) {
LOG_S(ERROR)<<e.what();
}
}
void Sensor::HandleOrders(WHISPER::Message WHmsg)
{
Orders::OrderType type = Orders::Order::getType(WHmsg);
switch (type) {
case Orders::HOLD_ORDER :
{
break;
}
default: {
case Orders::MOVE_ORDER :
{
break;
}
}
}
if (outgoingParentMessages->size() > 0) {
WHISPER::Message msg;
outgoingParentMessages->get(msg);
ParentUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]);
}
}
this->sendCalculatedDataIsRunnung = false;
}
void Sensor::SensorCalculations()
{
CalculationIsRunnung = true;
specificSensorCalculations();
while (!stopCalculationData) {
specificSensorCalculations();
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
LOG_S(INFO)<<"calculation is stopt";
CalculationIsRunnung = false;
}
void Sensor::ReloadCharacteristicts()
case Orders::ENGAGE_ORDER :
{
specificReloadCharacteristicts();
break;
}
case Orders::SYSTEM_STATE_ORDER :
{
break;
}
case Orders::UNKNOWN:
break;
}
}
void Sensor::setupOwnShip(std::shared_ptr<SimCore::SimTrack> ownShipTrack)
{
if(!ownShipTrack->getPosition().isValid())
{
return;
}
OwnShipTrack_ = ownShipTrack;
// LOG_S(INFO)<<"Own Ship Received";
}
void Sensor::updateOwnShipFunction()
{
while (stopOwnShipUpdater_ == false)
{
if (recognisedTracks_->size() > 0)
{
std::shared_ptr<SimCore::SimTrack> track;
recognisedTracks_->get(track);
if (track != nullptr)
{
LOG_S(INFO)<<"updated Tracklist";
client_->sendMessage(track->buildMessage().serialize());
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
}
void Sensor::handleGroundThruthMessage(std::string msg)
{
// LOG_S(INFO)<<"message received";
auto track = SimCore::SimTrack::unpack(msg);
if (track == nullptr)
{
return;
}
if (track->getIdentifier() == OwnShipID) {
return;
}
// LOG_S(INFO)<<"calling specific Sensor Calc";
specificSensorCalculations(std::move(track));
}