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

ADD: SensorManager with Trackfusion Trackstore and Sensor List wich contains every contact a sensor has

Browse Source
This commit is contained in:
Henry Winkel
2024-02-14 14:11:42 +01:00
parent cfe2aff5ce
commit 2a279f59ec
17 changed files with 678 additions and 425 deletions
Download Patch File Download Diff File Expand all files Collapse all files

25
src/Entities/Entity.cpp
View File

@@ -3,6 +3,7 @@
#include "Entities/SensorManager.hpp"
#include "Orders/MoveOrder.hpp"
#include "Orders/Order.hpp"
#include "Orders/TracklistRequest.hpp"
#include "SimCore/Messages/Control.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/UtilFunctions.hpp"
@@ -68,10 +69,9 @@ namespace Entities
MovemtServer_ = std::make_shared<DirectCommunication::DirectCommunicationServer>(__MOVEMENT_SERVER_PORT__,OwnID.getUUID());
CommandCommsServer_ = std::make_shared<DirectCommunication::DirectCommunicationServer>(CommandPort,OwnID.getUUID());
CommandCommsServer_->registerMessageCallback(std::bind(&Entity::handleExternalComms,this,std::placeholders::_1));
BroadcastServer_ = std::make_shared<WHISPER::InternalUDPSender>(GroundTruthAddr, GroundTruthPort);
BroadcastServer_ = std::make_shared<WHISPER::InternalUDPSender>(OwnID.getUUID(),GroundTruthAddr, GroundTruthPort);
@@ -98,7 +98,7 @@ namespace Entities
moveorder.setPosition(pos);
if(MovementWorkerStarted == true)
{
MovemtServer_->sendMessage(moveorder.buildMessage().serialize());
MovemtServer_->sendMessage(moveorder.buildMessage());
LOG_S(INFO)<<"Move Order send";
}
}
@@ -109,7 +109,7 @@ namespace Entities
moveorder.Speed.setValue(val);
if(MovementWorkerStarted == true)
{
MovemtServer_->sendMessage(moveorder.buildMessage().serialize());
MovemtServer_->sendMessage(moveorder.buildMessage());
LOG_S(INFO)<<"Move Order send with Speed";
}
}
@@ -120,7 +120,7 @@ namespace Entities
moveorder.Course.setValue(val);
if(MovementWorkerStarted == true)
{
MovemtServer_->sendMessage(moveorder.buildMessage().serialize());
MovemtServer_->sendMessage(moveorder.buildMessage());
LOG_S(INFO)<<"Move Order send with Course";
}
}
@@ -202,7 +202,7 @@ namespace Entities
LOG_S(INFO)<< "POS: LAT: "<< OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LATITUDE) << " LON: " << OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LONGITUDE);
MovementWorkerStarted = true;
MovemtServer_->sendMessage(OwnShipTrack->buildMessage().serialize());
MovemtServer_->sendMessage(OwnShipTrack->buildMessage());
LOG_S(INFO)<< "Initial Message send to MovementWorker";
}
@@ -292,8 +292,17 @@ namespace Entities
break;
}
case Orders::TRACKLIST_REQUEST:
{
{
auto TrackListRequest = Orders::TracklistRequest::unpack(whisperMsg);
if (TrackListRequest == nullptr)
{
break;
}
if (TrackListRequest->EntityID == OwnShipTrack->getIdentifier())
{
std::string senderUUID = whisperMsg.senderUUID_;
CommandCommsServer_->sendMessage(SensorManager_->getTrackListUpdate()->buildMessage().serialize(OwnShipTrack->getIdentifier().getUUID()),senderUUID);
}
break;
}
break;

2
src/Entities/Sensor.cpp
View File

@@ -197,7 +197,7 @@ void Sensor::updateOwnShipFunction()
if (track != nullptr)
{
LOG_S(INFO)<<"updated Tracklist";
client_->sendMessage(track->buildMessage().serialize());
client_->sendMessage(track->buildMessage());
}
}

35
src/Entities/SensorControl.cpp
View File

@@ -1,4 +1,6 @@
#include <SimCore/Messages/SensorData.hpp>
#include "Entities/Tracklist/Tracklist.hpp"
#include "SimCore/Identifier.hpp"
#include "nlohmann/json_fwd.hpp"
#include <Entities/SensorControl.hpp>
#include <memory>
@@ -8,41 +10,14 @@
namespace Entities
namespace Sensor
{
SensorControl::SensorControl(std::string ID, std::string Name, std::string IP, SimCore::SensorKinds sensorKind):
ID_(ID),IP_(IP),Name_(Name),SensorKind_(sensorKind)
SensorControl::SensorControl(SimCore::Identifier ID, std::string Name, std::string IP, SimCore::SensorKinds sensorKind)
:SimCore::SensorData(ID,Name,IP,sensorKind)
{
TrackStore = std::make_unique<TrackList::TrackList>();
}
std::string SensorControl::getName()
{
return Name_;
}
std::string SensorControl::getID()
{
return ID_;
}
SimCore::SensorKinds SensorControl::getSensorKind()
{
return SensorKind_;
}
void SensorControl::updateStatus(SimCore::Status status)
{
SensorSatus_ = status;
}
SimCore::Status SensorControl::getSensorStatus()
{
return SensorSatus_;
}

129
src/Entities/SensorManager.cpp
View File

@@ -1,4 +1,5 @@
#include "Entities/SensorControl.hpp"
#include "SimCore/Identifier.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/SimCore.hpp"
#include "WHISPER/Messages/Message.hpp"
@@ -21,7 +22,7 @@ namespace Entities
void SensorManager::startSensor(std::shared_ptr<kubecontrol::KubePod> pod,SimCore::SensorKinds SensorKind)
{
PodController_->startPod(pod);
SensorStore[pod->getUUID()] = std::make_unique<Entities::SensorControl>(pod->getUUID(),pod->getName(),pod->getIp(),SensorKind);
SensorStore[pod->getUUID()] = std::make_unique<Sensor::SensorControl>(SimCore::Identifier(pod->getUUID()),pod->getName(),pod->getIp(),SensorKind);
}
@@ -30,6 +31,14 @@ namespace Entities
//TODO send stop message to sensor
}
void SensorManager::stopAllSensors()
{
//TODO send stop message to sensor
}
void SensorManager::deleteSensor(std::string uuid)
{
@@ -38,18 +47,55 @@ namespace Entities
}
void SensorManager::deleteAllSensor()
{
PodController_->stopAllPods();
SensorStore.clear();
}
std::shared_ptr<Sensor::SensorControl> SensorManager::getSensorByUUID(std::string uuid)
{
auto it = SensorStore.find(uuid);
if( it != SensorStore.end())
{
return it->second;
}
return nullptr;
}
void SensorManager::addSensorLocal(std::unique_ptr<Sensor::SensorControl> sensor)
{
SensorStore[sensor->getID().getUUID()] = std::move(sensor);
}
size_t SensorManager::getSensorCount()
{
return SensorStore.size();
}
void SensorManager::handlSensorMessages(std::string Message)
{
WHISPER::Message msg(Message);
WHISPER::Message msg(Message);
switch (msg.msgType_)
{
case WHISPER::MsgType::SIM_TRACK:
{
{
auto track = std::make_shared<SimCore::SimTrack>(SimCore::SimTrack::unpack(msg));
updateTracklistForSensor(msg.senderUUID_, track);
if(track != nullptr)
{
updateTracklistForSensor(msg.senderUUID_, track);
auto sensor = getSensorByUUID(msg.senderUUID_);
Trackfusion_.addOrUpdateTrack(track, sensor);
}
break;
}
case WHISPER::MsgType::SYSTEMSTATE_UPDATE:
@@ -66,30 +112,53 @@ namespace Entities
void SensorManager::updateTracklistForSensor(std::string uuidSensor, std::shared_ptr<SimCore::SimTrack> track)
{
auto it = SensorStore.find(uuidSensor);
it->second->TrackStore->addTrack(track);
auto sensor= getSensorByUUID(uuidSensor);
if(sensor == nullptr)
{
return;
}
sensor->TrackStore.addTrack(std::move(track));
// auto it = SensorStore.find(uuidSensor);
// if( it != SensorStore.end())
// {
// it->second->TrackStore.addTrack(std::move(track));
// }
}
void SensorManager::sendMessageToSensor(std::string message, std::string uuid )
{
if (uuid.empty())
{
SensorServer_->sendMessage(message);
}else {
SensorServer_->sendMessage(message,uuid);
}
}
std::string SensorManager::getTracklistStringBySensor()
{
nlohmann::json result;
nlohmann::json radar;
radar["SensorType"] = SimCore::toString(SimCore::SensorKinds::RADAR);
nlohmann::json esm;
esm["SensorType"] = SimCore::toString(SimCore::SensorKinds::ESM);
nlohmann::json sonar;
sonar["SensorType"] = SimCore::toString(SimCore::SensorKinds::SONAR);
nlohmann::json visual;
visual["SensorType"] = SimCore::toString(SimCore::SensorKinds::VISUAL);
for(const auto& [key, value] : SensorStore )
{
nlohmann::json sensor;
sensor["SensorName"] = value->getName();
sensor["SensorID"] = value->getID();
sensor["SensorID"] = value->getID().getUUID();
sensor["SensorStatus"] = SimCore::toString(value->getSensorStatus());
sensor["SensorEnabled"] = "false";
if (value->getSensorStatus() == SimCore::Status::ACTIVE)
@@ -102,57 +171,73 @@ namespace Entities
sensor["SensorDamaged"] = "true";
}
nlohmann::json contacts;
value->TrackStore->getJsonTRackList(contacts);
sensor["SensorContacts"].push_back(contacts);
value->TrackStore.getJsonTRackList(contacts);
sensor["SensorContacts"] = contacts["Entities"];
switch (value->getSensorKind()) {
case SimCore::SensorKinds::RADAR:
{
radar["Sensors"].push_back(sensor);
radar.push_back(sensor);
break;
}
case SimCore::SensorKinds::ESM :
{
esm["Sensors"].push_back(sensor);
esm.push_back(sensor);
break;
}
case SimCore::SensorKinds::SONAR :
{
sonar["Sensors"].push_back(sensor);
sonar.push_back(sensor);
break;
}
case SimCore::SensorKinds::VISUAL :
{
visual["Sensors"].push_back(sensor);
visual.push_back(sensor);
break;
}
}
result.push_back(radar);
result.push_back(esm);
result.push_back(sonar);
result.push_back(visual);
// result.push_back(esm);
// result.push_back(sonar);
// result.push_back(visual);
}
result[(int)SimCore::SensorKinds::RADAR]["SensorType"] = "Radar";
result[(int)SimCore::SensorKinds::RADAR]["Sensors"]= radar;
result[(int)SimCore::SensorKinds::ESM]["SensorType"] = SimCore::toString(SimCore::SensorKinds::ESM);
result[(int)SimCore::SensorKinds::ESM]["Sensors"] = esm;
result[(int)SimCore::SensorKinds::SONAR]["SensorType"] = SimCore::toString(SimCore::SensorKinds::SONAR);
result[(int)SimCore::SensorKinds::SONAR]["Sensors"] = sonar;
result[(int)SimCore::SensorKinds::VISUAL]["SensorType"] = SimCore::toString(SimCore::SensorKinds::VISUAL);
result[(int)SimCore::SensorKinds::VISUAL]["Sensors"] = visual;
return result.dump();
}
std::string SensorManager::getTracklistStringFusioned()
{
std::string result = Trackfusion_.getTracklistAsString();
return result;
}
std::unique_ptr<SimCore::TracklistUpdate> SensorManager::getTrackListUpdate()
{
return std::move(Trackfusion_.getTrackListUpdate(OwnId_));
}
}

129
src/Entities/Tracklist/TrackListItem.cpp
View File

@@ -1,129 +0,0 @@
#include "SimCore/Messages/SimTrack.hpp"
#include <SimCore/SimCore.hpp>
#include <Entities/Tracklist/TracklistItem.hpp>
#include <algorithm>
#include <cstddef>
#include <memory>
#include <chrono>
#include <tuple>
namespace TrackList {
TracklistItem::TracklistItem(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData):
SimCore::SimTrack(*track.get())
{
addSensorDataToSensorList(sensorData);
lastUpdateTimestamp_ = std::chrono::steady_clock::now();
}
TracklistItem::TracklistItem(std::shared_ptr<SimCore::SimTrack> track):
SimCore::SimTrack(*track.get())
{
updateTrack(track);
lastUpdateTimestamp_ = std::chrono::steady_clock::now();
}
std::chrono::time_point<std::chrono::steady_clock> TracklistItem::getLastUpdateTimestamp()
{
return lastUpdateTimestamp_;
}
void TracklistItem::updateTrack(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData)
{
if(this->getIdentifier() == track->getIdentifier())
{
this->setPosition(track->getPosition());
this->Speed.setValue(track->Speed.getValue());
this->Course.setValue(track->Course.getValue());
}
if (!isSensorinSensorlist(sensorData))
{
SensorList.push_back(sensorData);
}
lastUpdateTimestamp_ = std::chrono::steady_clock::now();
}
void TracklistItem::updateTrack(std::shared_ptr<SimCore::SimTrack> track )
{
if(this->getIdentifier() == track->getIdentifier())
{
this->setPosition(track->getPosition());
this->Speed.setValue(track->Speed.getValue());
this->Course.setValue(track->Course.getValue());
}
lastUpdateTimestamp_ = std::chrono::steady_clock::now();
}
size_t TracklistItem::getSensorCount()
{
return SensorList.size();
}
bool TracklistItem::isSensorinSensorlist(SensorData sensorData)
{
auto it = std::find(SensorList.begin(),SensorList.end(), sensorData);
if (it != SensorList.end()) {
return true;
} else {
return false;
}
}
bool TracklistItem::isSensorIDKnown(SimCore::Identifier SensorID)
{
for (auto i: SensorList) {
if (i.sensorID == SensorID)
{
return true;
}
}
return false;
}
void TracklistItem::addSensorDataToSensorList(SensorData sensorData)
{
if (isSensorinSensorlist(sensorData) == false) {
SensorList.emplace_back(sensorData);
}
}
bool TracklistItem::checkIfSensorIDIsIn(SimCore::Identifier SensorID)
{
for (auto i:SensorList) {
if (i.sensorID == SensorID) {
return true;
}
}
return false;
}
}

131
src/Entities/Tracklist/Trackfusion.cpp Normal file
View File

@@ -0,0 +1,131 @@
#include <SimCore/Messages/TracklistItem.hpp>
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/Messages/TracklistUpdate.hpp"
#include "nlohmann/json_fwd.hpp"
#include <Entities/Tracklist/Trackfusion.hpp>
#include <memory>
#include <mutex>
namespace TrackList
{
void Trackfusion::addOrUpdateTrack(std::shared_ptr<SimCore::SimTrack> track, std::shared_ptr<SimCore::SensorData> sensorData)
{
std::lock_guard<std::mutex> lock(mx_);
auto iterator = TrackStore_.find(track->getIdentifier());
if (iterator == TrackStore_.end())
{
auto Item = std::make_shared<SimCore::TracklistItem>(track,*sensorData);
TrackStore_[Item->getIdentifier()] = Item;
}else {
iterator->second->updateTrack(track);
iterator->second->addSensorDataToSensorList(*sensorData);
}
}
void Trackfusion::deleteTrack(SimCore::Identifier id)
{
std::lock_guard<std::mutex> lock(mx_);
auto it = TrackStore_.find(id);
if (it != TrackStore_.end())
{
it = TrackStore_.erase(it);
}
}
void Trackfusion::deleteTrack(std::string uuid)
{
std::lock_guard<std::mutex> lock(mx_);
for (auto it = TrackStore_.begin(); it != TrackStore_.end();)
{
if (it->first.getUUID() == uuid)
{
it = TrackStore_.erase(it);
}
else{
++it;
}
}
}
std::shared_ptr<SimCore::TracklistItem> Trackfusion::findTrack(SimCore::Identifier id)
{
std::lock_guard<std::mutex> lock(mx_);
auto it = TrackStore_.find(id);
if (it != TrackStore_.end())
{
return it->second;
}
return nullptr;
}
std::shared_ptr<SimCore::TracklistItem> Trackfusion::findTrack(std::string uuid)
{
std::lock_guard<std::mutex> lock(mx_);
for (auto it = TrackStore_.begin(); it != TrackStore_.end();)
{
if (it->first.getUUID() == uuid)
{
return it->second;
}
}
return nullptr;
}
std::string Trackfusion::getTracklistAsString()
{
std::lock_guard<std::mutex> lock(mx_);
nlohmann::json j;
LOG_S(INFO)<<" Trackstore size: " <<TrackStore_.size();
for (auto it :TrackStore_)
{
j.push_back(it.second->getsTrackListItemAsJSON());
}
return j.dump();
}
std::unique_ptr<SimCore::TracklistUpdate> Trackfusion::getTrackListUpdate(SimCore::Identifier ID)
{
auto update = std::make_unique<SimCore::TracklistUpdate>(ID);
for (auto it :TrackStore_)
{
// update->addTrack(*it.second);
}
return std::move(update);
}
}