ADD: updated Sensor class
This commit is contained in:
Henry Winkel
@@ -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>>();
|
||||
|
||||
recognisedTracks_ = std::make_shared<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>>();
|
||||
|
||||
incommingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
|
||||
outgoingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
|
||||
|
||||
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,197 +52,159 @@ 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);
|
||||
|
||||
stopCalculationData = false;
|
||||
sensorCalculationThread = std::thread(&Sensor::SensorCalculations,this);
|
||||
this->stopOwnShipUpdater_ = false;
|
||||
UpdateOwnShipThread_ = std::thread(&Sensor::updateOwnShipFunction,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()
|
||||
{
|
||||
this->ReceivingGroundThruthIsRunnung = true;
|
||||
GroundTruthUDPService_->connect(incommingGroundThruthMessages);
|
||||
GroundTruthUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::TRACK]);
|
||||
void Sensor::handlServerMessages(std::string msg)
|
||||
{
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
try {
|
||||
WHISPER::Message whisperMsg(msg);
|
||||
// LOG_S(INFO)<<"New Message from TCP Client";
|
||||
// LOG_S(INFO)<<"Message Type is: " << whisperMsg.msgType_;
|
||||
|
||||
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()
|
||||
{
|
||||
this->sendCalculatedDataIsRunnung = true;
|
||||
|
||||
ParentUDPService_->connect(incommingParentMessages);
|
||||
ParentUDPService_->subscribe(WHISPER::MsgTopicsMap[WHISPER::MsgTopics::COMMANDS]);
|
||||
|
||||
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;
|
||||
|
||||
switch (msg.msgType_) {
|
||||
case WHISPER::MsgType::OWN_TRACK :{
|
||||
|
||||
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: {
|
||||
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
switch (whisperMsg.msgType_)
|
||||
{
|
||||
case WHISPER::MsgType::ORDER:
|
||||
{
|
||||
|
||||
LOG_S(INFO)<<"ORDER";
|
||||
HandleOrders(whisperMsg);
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
case WHISPER::MsgType::SIM_TRACK:
|
||||
{
|
||||
|
||||
OwnShipTrack_ = SimCore::SimTrack::unpack(msg);
|
||||
if (OwnShipTrack_ != nullptr)
|
||||
{
|
||||
// LOG_S(INFO)<<"own SHip data received";
|
||||
setupOwnShip(OwnShipTrack_);
|
||||
|
||||
|
||||
|
||||
if (outgoingParentMessages->size() > 0) {
|
||||
WHISPER::Message msg;
|
||||
outgoingParentMessages->get(msg);
|
||||
ParentUDPService_->publish(msg.serialize(), WHISPER::MsgTopicsMap[(WHISPER::MsgTopics)msg.topic_]);
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
this->sendCalculatedDataIsRunnung = false;
|
||||
|
||||
|
||||
} 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 :
|
||||
{
|
||||
|
||||
void Sensor::SensorCalculations()
|
||||
{
|
||||
CalculationIsRunnung = true;
|
||||
specificSensorCalculations();
|
||||
break;
|
||||
}
|
||||
case Orders::MOVE_ORDER :
|
||||
{
|
||||
|
||||
break;
|
||||
}
|
||||
case Orders::ENGAGE_ORDER :
|
||||
{
|
||||
|
||||
while (!stopCalculationData) {
|
||||
specificSensorCalculations();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(500));
|
||||
break;
|
||||
}
|
||||
case Orders::SYSTEM_STATE_ORDER :
|
||||
{
|
||||
|
||||
}
|
||||
LOG_S(INFO)<<"calculation is stopt";
|
||||
break;
|
||||
}
|
||||
case Orders::UNKNOWN:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
CalculationIsRunnung = false;
|
||||
}
|
||||
|
||||
void Sensor::ReloadCharacteristicts()
|
||||
void Sensor::setupOwnShip(std::shared_ptr<SimCore::SimTrack> ownShipTrack)
|
||||
{
|
||||
if(!ownShipTrack->getPosition().isValid())
|
||||
{
|
||||
|
||||
specificReloadCharacteristicts();
|
||||
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));
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user