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

MOD: made modification to fit to SIMCORE

Browse Source
This commit is contained in:
Henry Winkel
2023-08-10 10:47:15 +02:00
parent 9c8ffad475
commit 330bff31fb
9 changed files with 62 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
include/Entities/Entity.hpp
View File

@@ -4,9 +4,7 @@
#include "DirectCommunicationServer.hpp" #include "DirectCommunicationServer.hpp"
#include "Entities/Movement.hpp" #include "Entities/Movement.hpp"
#include "SimCore/Messages/GroundThruthTrack.hpp"
#include "SimCore/Messages/SimTrack.hpp" #include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/Messages/Track.hpp"
#include "SimCore/Orientation.hpp" #include "SimCore/Orientation.hpp"
#include "SimCore/SafeMap.hpp" #include "SimCore/SafeMap.hpp"
#include "SimCore/SimCore.hpp" #include "SimCore/SimCore.hpp"
@@ -55,7 +53,6 @@ namespace Entities {
Entity(const SimCore::Identifier OwnID, Entity(const SimCore::Identifier OwnID,
std::string EnttityName, std::string EnttityName,
WHISPER::SourceType OwnType, WHISPER::SourceType OwnType,
SimCore::Identifier ParentID,
SimCore::EntityKind EntityKind, SimCore::EntityKind EntityKind,
std::uint32_t GroundTruthPort, std::uint32_t GroundTruthPort,
ushort CommandPort, ushort CommandPort,
@@ -82,7 +79,6 @@ namespace Entities {
std::shared_ptr<SimCore::SimTrack> OwnShipTrack = nullptr; std::shared_ptr<SimCore::SimTrack> OwnShipTrack = nullptr;
std::string EntityName_; std::string EntityName_;
SimCore::Identifier ParentID_;
SimCore::EntityKind EntityKind_; SimCore::EntityKind EntityKind_;
ushort MovemntWorkerPort_; ushort MovemntWorkerPort_;
@@ -106,7 +102,7 @@ namespace Entities {
void handleMovement(); void handleMovement();
std::shared_ptr<DirectCommunication::DirectCommunicationServer> ExternalCommsServer_ = nullptr; std::shared_ptr<DirectCommunication::DirectCommunicationServer> CommandCommsServer_ = nullptr;
void handleExternalComms(std::string msg); void handleExternalComms(std::string msg);
@@ -120,7 +116,7 @@ namespace Entities {
std::shared_ptr<std::list<Entities::SensorClientData>> SensorStore_; std::shared_ptr<std::list<Entities::SensorClientData>> SensorStore_;
std::shared_ptr<SimCore::SafeMap<SimCore::Identifier, std::shared_ptr<SimCore::Track>>> Trackstore_; std::shared_ptr<SimCore::SafeMap<SimCore::Identifier, std::shared_ptr< SimCore::SimTrack>>> Trackstore_;
}; };

7
include/Entities/Sensor.hpp
View File

@@ -1,9 +1,6 @@
#pragma once #pragma once
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/Messages/GroundThruthTrack.hpp"
#include "SimCore/Messages/Track.hpp"
#include <WHISPER/InternalUDPService.hpp> #include <WHISPER/InternalUDPService.hpp>
#include <SimCore/Identifier.hpp> #include <SimCore/Identifier.hpp>
@@ -25,7 +22,7 @@ namespace Entities {
void stop(); void stop();
protected: protected:
std::shared_ptr<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::GroundTruthTrack>>> incommingTrackMessages = nullptr; 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>> incommingGroundThruthMessages = nullptr;

10
include/Entities/Tracklist/Tracklist.hpp
View File

@@ -3,13 +3,13 @@
#include "SimCore/Identifier.hpp" #include "SimCore/Identifier.hpp"
#include "SimCore/IdentifierMaker.hpp" #include "SimCore/IdentifierMaker.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include <atomic> #include <atomic>
#include <cstddef> #include <cstddef>
#include <list> #include <list>
#include <memory> #include <memory>
#include <SimCore/Position.hpp> #include <SimCore/Position.hpp>
#include <SimCore/Messages/Track.hpp>
#include <SimCore/SafeMap.hpp> #include <SimCore/SafeMap.hpp>
#include <Entities/Tracklist/TracklistItem.hpp> #include <Entities/Tracklist/TracklistItem.hpp>
#include <thread> #include <thread>
@@ -28,9 +28,9 @@ namespace TrackList
SimCore::Identifier getTrackID(SimCore::ObjectSource source); SimCore::Identifier getTrackID(SimCore::ObjectSource source);
void addTrack(std::shared_ptr<SimCore::Track> track); void addTrack(std::shared_ptr<SimCore::SimTrack> track);
void addTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData); void addTrack(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData);
std::shared_ptr<TracklistItem> getTrack(SimCore::Identifier TrackID); std::shared_ptr<TracklistItem> getTrack(SimCore::Identifier TrackID);
@@ -46,8 +46,8 @@ namespace TrackList
void tracklistSanitizer(); void tracklistSanitizer();
void addNewTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData); void addNewTrack(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData);
void addNewTrack(std::shared_ptr<SimCore::Track> track); void addNewTrack(std::shared_ptr<SimCore::SimTrack> track);
std::map<std::string, std::shared_ptr<TracklistItem>> TrackList_; std::map<std::string, std::shared_ptr<TracklistItem>> TrackList_;

10
include/Entities/Tracklist/TracklistItem.hpp
View File

@@ -2,7 +2,7 @@
#include "SimCore/Identifier.hpp" #include "SimCore/Identifier.hpp"
#include "SimCore/Messages/Track.hpp" #include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/SimCore.hpp" #include "SimCore/SimCore.hpp"
#include <SimCore/Position.hpp> #include <SimCore/Position.hpp>
#include <chrono> #include <chrono>
@@ -31,8 +31,8 @@ namespace TrackList {
class TracklistItem class TracklistItem
{ {
public: public:
TracklistItem(std::shared_ptr<SimCore::Track> track,SensorData sensorData); TracklistItem(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData);
TracklistItem(std::shared_ptr<SimCore::Track> track); TracklistItem(std::shared_ptr<SimCore::SimTrack> track);
SimCore::Identifier getID(); SimCore::Identifier getID();
@@ -55,9 +55,9 @@ namespace TrackList {
std::chrono::time_point<std::chrono::steady_clock> getLastUpdateTimestamp(); std::chrono::time_point<std::chrono::steady_clock> getLastUpdateTimestamp();
void updateTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData); void updateTrack(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData);
void updateTrack(std::shared_ptr<SimCore::Track> track); void updateTrack(std::shared_ptr<SimCore::SimTrack> track);
bool checkIfSensorIDIsIn(SimCore::Identifier SensorTrackID); bool checkIfSensorIDIsIn(SimCore::Identifier SensorTrackID);

2
libs/SimCore

Submodule libs/SimCore updated: 5e8b1b84e8...ed14aedf0c

29
src/Entities/Entity.cpp
View File

@@ -25,7 +25,7 @@
#define __MOVEMENT_SERVER_PORT__ 5556 #define __MOVEMENT_SERVER_PORT__ 5556
#define __EXTERNAL_SERVER_PORT__ 5557 #define __SENSOR_SERVER_PORT__ 5557
@@ -35,13 +35,11 @@ namespace Entities
Entity::Entity(const SimCore::Identifier OwnID, Entity::Entity(const SimCore::Identifier OwnID,
std::string EnttityName, std::string EnttityName,
WHISPER::SourceType OwnType, WHISPER::SourceType OwnType,
SimCore::Identifier ParentID,
SimCore::EntityKind EntityKind, SimCore::EntityKind EntityKind,
std::uint32_t GroundTruthPort, std::uint32_t GroundTruthPort,
ushort CommandPort, ushort CommandPort,
bool online): bool online):
EntityName_(EnttityName), EntityName_(EnttityName),
ParentID_(ParentID),
EntityKind_(EntityKind), EntityKind_(EntityKind),
online_(online) online_(online)
@@ -52,9 +50,9 @@ namespace Entities
MovemtServer_ = std::make_shared<DirectCommunication::DirectCommunicationServer>(__MOVEMENT_SERVER_PORT__); MovemtServer_ = std::make_shared<DirectCommunication::DirectCommunicationServer>(__MOVEMENT_SERVER_PORT__);
// ExternalCommsServer_ = std::make_shared<DirectCommunication::DirectCommunicationServer>(8000); CommandCommsServer_ = std::make_shared<DirectCommunication::DirectCommunicationServer>(CommandPort);
// ExternalCommsServer_->registerMessageCallback(std::bind(&Entity::handleExternalComms,this,std::placeholders::_1)); CommandCommsServer_->registerMessageCallback(std::bind(&Entity::handleExternalComms,this,std::placeholders::_1));
} }
@@ -62,6 +60,7 @@ namespace Entities
{ {
PodController_->stopAllPods(); PodController_->stopAllPods();
stop(); stop();
MovemtServer_.reset();
} }
@@ -187,13 +186,26 @@ namespace Entities
// LOG_S(INFO)<< "new POS: LAT: "<< OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LATITUDE) << " LON: " << OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LONGITUDE); // LOG_S(INFO)<< "new POS: LAT: "<< OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LATITUDE) << " LON: " << OwnShipTrack->getPosition().getGeodesicPos()(SimCore::LONGITUDE);
} }
} }
if (MovemtServer_->countClients() == 0)
{
MovementWorkerStarted = false;
}
} }
void Entity::handleExternalComms(std::string msg) void Entity::handleExternalComms(std::string msg)
{ {
LOG_S(INFO)<<msg; LOG_S(INFO)<<msg;
auto MoveOrder = Orders::MoveOrder::unpack(msg);
LOG_S(INFO)<<this->ExternalCommsServer_->countClients(); if (MoveOrder != nullptr)
{
if(MoveOrder->Speed.isValid()) this->setSpeed(MoveOrder->Speed.getValue());
}
LOG_S(INFO)<<this->CommandCommsServer_->countClients();
CommandCommsServer_->sendMessage("Hello Client");
} }
@@ -202,14 +214,11 @@ namespace Entities
void Entity::MainLoop() void Entity::MainLoop()
{ {
auto server = DirectCommunication::DirectCommunicationServer(35000);
LOG_S(INFO)<< "main loop started"; LOG_S(INFO)<< "main loop started";
while (!stopMainLoop) while (!stopMainLoop)
{ {
handleMovement(); handleMovement();
LOG_S(INFO)<<server.countClients();
server.sendMessage("hello client");
LOG_S(INFO)<<"hello";
std::this_thread::sleep_for(std::chrono::milliseconds(500)); std::this_thread::sleep_for(std::chrono::milliseconds(500));

17
src/Entities/Sensor.cpp
View File

@@ -1,6 +1,5 @@
#include "SimCore/Messages/GroundThruthTrack.hpp" #include <SimCore/Position.hpp>
#include "SimCore/Messages/Track.hpp" #include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/Position.hpp"
#include "SimCore/SimCore.hpp" #include "SimCore/SimCore.hpp"
#include "SimCore/UtilFunctions.hpp" #include "SimCore/UtilFunctions.hpp"
#include "WHISPER/Messages/Message.hpp" #include "WHISPER/Messages/Message.hpp"
@@ -24,11 +23,11 @@ namespace Entities {
incommingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>(); incommingParentMessages = std::make_shared<WHISPER::threadSafeQueue<WHISPER::Message>>();
outgoingParentMessages = 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::GroundTruthTrack>>>(); incommingTrackMessages = std::make_shared<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>>();
GroundTruthUDPService_ = std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,GroundTruthPort_,SimCore::UtilFunctions::implode(ip,'.'),ownIP); // 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); // ParentUDPService_ = std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,ParentPort,ParentIPAddress_,ownIP);
}; };
@@ -122,7 +121,7 @@ namespace Entities {
WHISPER::Message msg; WHISPER::Message msg;
incommingGroundThruthMessages->get(msg); incommingGroundThruthMessages->get(msg);
if (msg.msgType_ == WHISPER::MsgType::GROUND_TRUTH_TRACK) { if (msg.msgType_ == WHISPER::MsgType::GROUND_TRUTH_TRACK) {
auto GTrack = std::make_shared<SimCore::GroundTruthTrack>(std::move(SimCore::GroundTruthTrack::unpack(msg))); auto GTrack = std::make_shared<SimCore::SimTrack>(std::move(SimCore::SimTrack::unpack(msg)));
incommingTrackMessages->addElement(GTrack); incommingTrackMessages->addElement(GTrack);
} }
} }
@@ -156,9 +155,9 @@ namespace Entities {
switch (msg.msgType_) { switch (msg.msgType_) {
case WHISPER::MsgType::OWN_TRACK :{ case WHISPER::MsgType::OWN_TRACK :{
auto OwnTrack = SimCore::GroundTruthTrack::unpack(msg); auto OwnTrack = SimCore::SimTrack::unpack(msg);
// SimCore::Track OwnTrack(msg.serialize()); // SimCore::Track OwnTrack(msg.serialize());
auto tmpPos = OwnTrack.getPostion().getGeocentricPos(); auto tmpPos = OwnTrack.getPosition().getGeocentricPos();
if (this->ownShipPosition_ == nullptr) { if (this->ownShipPosition_ == nullptr) {
this->ownShipPosition_ = std::make_shared<SimCore::Position>( this->ownShipPosition_ = std::make_shared<SimCore::Position>(
tmpPos[SimCore::GeocentricPosition::X], tmpPos[SimCore::GeocentricPosition::X],

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

@@ -1,4 +1,4 @@
#include <SimCore/Messages/RadarTrack.hpp> #include "SimCore/Messages/SimTrack.hpp"
#include <SimCore/SimCore.hpp> #include <SimCore/SimCore.hpp>
#include <Entities/Tracklist/TracklistItem.hpp> #include <Entities/Tracklist/TracklistItem.hpp>
@@ -9,7 +9,7 @@
namespace TrackList { namespace TrackList {
TracklistItem::TracklistItem(std::shared_ptr<SimCore::Track> track,SensorData sensorData):trackID_(track->getIdentifier()) TracklistItem::TracklistItem(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData):trackID_(track->getIdentifier())
{ {
updateTrack(track,sensorData); updateTrack(track,sensorData);
@@ -17,7 +17,7 @@ namespace TrackList {
} }
TracklistItem::TracklistItem(std::shared_ptr<SimCore::Track> track):trackID_(track->getIdentifier()) TracklistItem::TracklistItem(std::shared_ptr<SimCore::SimTrack> track):trackID_(track->getIdentifier())
{ {
updateTrack(track); updateTrack(track);
@@ -89,22 +89,17 @@ namespace TrackList {
} }
void TracklistItem::updateTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData) void TracklistItem::updateTrack(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData)
{ {
auto trackKind = track->getTrackkind();
if (trackKind == SimCore::TrackKind::RADAR_TRACK | trackKind == SimCore::TrackKind::GROUND_TRUTH_TRACK) {
std::shared_ptr<SimCore::RadarTrack> radarTrack = std::dynamic_pointer_cast<SimCore::RadarTrack>(track);
position_ = radarTrack->getPostion(); position_ = track->getPosition();
range_ = radarTrack->getRange(); course_ = track->Course.getValue();
bearing_ = radarTrack->getBearing(); speed_ = track->Speed.getValue();
course_ = radarTrack->getCourse();
speed_ = radarTrack->getSpeed();
lastUpdateTimestamp_ = std::chrono::steady_clock::now(); lastUpdateTimestamp_ = std::chrono::steady_clock::now();
}
if (isSensorinSensorlist(sensorData) != true) { if (isSensorinSensorlist(sensorData) != true) {
addSensorDataToSensorList(sensorData); addSensorDataToSensorList(sensorData);
@@ -112,23 +107,15 @@ namespace TrackList {
} }
void TracklistItem::updateTrack(std::shared_ptr<SimCore::Track> track ) void TracklistItem::updateTrack(std::shared_ptr<SimCore::SimTrack> track )
{ {
auto trackKind = track->getTrackkind(); position_ = track->getPosition();
if (trackKind == SimCore::TrackKind::RADAR_TRACK | trackKind == SimCore::TrackKind::GROUND_TRUTH_TRACK) { course_ = track->Course.getValue();
std::shared_ptr<SimCore::RadarTrack> radarTrack = std::dynamic_pointer_cast<SimCore::RadarTrack>(track); speed_ = track->Speed.getValue();
position_ = radarTrack->getPostion();
range_ = radarTrack->getRange();
bearing_ = radarTrack->getBearing();
course_ = radarTrack->getCourse();
speed_ = radarTrack->getSpeed();
lastUpdateTimestamp_ = std::chrono::steady_clock::now(); lastUpdateTimestamp_ = std::chrono::steady_clock::now();
}
} }

11
src/Entities/Tracklist/Tracklist.cpp
View File

@@ -1,3 +1,4 @@
#include "SimCore/Messages/SimTrack.hpp"
#include <Entities/Tracklist/Tracklist.hpp> #include <Entities/Tracklist/Tracklist.hpp>
@@ -34,11 +35,11 @@ namespace TrackList
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
return *IDMaker.getNewIdentifier(OwnID_.getNumber(), source).get(); return *IDMaker.getNewIdentifier(source).get();
} }
void TrackList::addTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData) void TrackList::addTrack(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData)
{ {
auto AllIDs = getAllIDs(); auto AllIDs = getAllIDs();
@@ -76,7 +77,7 @@ namespace TrackList
/// @brief /// @brief
/// @param track /// @param track
void TrackList::addTrack(std::shared_ptr<SimCore::Track> track) void TrackList::addTrack(std::shared_ptr<SimCore::SimTrack> track)
{ {
auto AllIDs = getAllIDs(); auto AllIDs = getAllIDs();
@@ -116,7 +117,7 @@ namespace TrackList
// lock.unlock(); // lock.unlock();
} }
void TrackList::addNewTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData) void TrackList::addNewTrack(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData)
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
@@ -126,7 +127,7 @@ namespace TrackList
TrackList_.emplace(id,item); TrackList_.emplace(id,item);
} }
void TrackList::addNewTrack(std::shared_ptr<SimCore::Track> track) void TrackList::addNewTrack(std::shared_ptr<SimCore::SimTrack> track)
{ {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);