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

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

@@ -3,13 +3,13 @@
#include "SimCore/Identifier.hpp"
#include "SimCore/IdentifierMaker.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include <atomic>
#include <cstddef>
#include <list>
#include <memory>
#include <SimCore/Position.hpp>
#include <SimCore/Messages/Track.hpp>
#include <SimCore/SafeMap.hpp>
#include <Entities/Tracklist/TracklistItem.hpp>
#include <thread>
@@ -28,9 +28,9 @@ namespace TrackList
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);
@@ -46,8 +46,8 @@ namespace TrackList
void tracklistSanitizer();
void addNewTrack(std::shared_ptr<SimCore::Track> track,SensorData sensorData);
void addNewTrack(std::shared_ptr<SimCore::Track> track);
void addNewTrack(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData);
void addNewTrack(std::shared_ptr<SimCore::SimTrack> track);
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/Messages/Track.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/SimCore.hpp"
#include <SimCore/Position.hpp>
#include <chrono>
@@ -31,8 +31,8 @@ namespace TrackList {
class TracklistItem
{
public:
TracklistItem(std::shared_ptr<SimCore::Track> track,SensorData sensorData);
TracklistItem(std::shared_ptr<SimCore::Track> track);
TracklistItem(std::shared_ptr<SimCore::SimTrack> track,SensorData sensorData);
TracklistItem(std::shared_ptr<SimCore::SimTrack> track);
SimCore::Identifier getID();
@@ -55,9 +55,9 @@ namespace TrackList {
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);

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 __EXTERNAL_SERVER_PORT__ 5557
#define __SENSOR_SERVER_PORT__ 5557
@@ -35,13 +35,11 @@ namespace Entities
Entity::Entity(const SimCore::Identifier OwnID,
std::string EnttityName,
WHISPER::SourceType OwnType,
SimCore::Identifier ParentID,
SimCore::EntityKind EntityKind,
std::uint32_t GroundTruthPort,
ushort CommandPort,
bool online):
EntityName_(EnttityName),
ParentID_(ParentID),
EntityKind_(EntityKind),
online_(online)
@@ -52,9 +50,9 @@ namespace Entities
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();
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);
}
}
if (MovemtServer_->countClients() == 0)
{
MovementWorkerStarted = false;
}
}
void Entity::handleExternalComms(std::string 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()
{
auto server = DirectCommunication::DirectCommunicationServer(35000);
LOG_S(INFO)<< "main loop started";
while (!stopMainLoop)
{
handleMovement();
LOG_S(INFO)<<server.countClients();
server.sendMessage("hello client");
LOG_S(INFO)<<"hello";
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/Messages/Track.hpp"
#include "SimCore/Position.hpp"
#include <SimCore/Position.hpp>
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/SimCore.hpp"
#include "SimCore/UtilFunctions.hpp"
#include "WHISPER/Messages/Message.hpp"
@@ -24,11 +23,11 @@ namespace Entities {
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::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);
ParentUDPService_ = std::make_shared<WHISPER::InternalUDPService>(OwnID.getParentNumber(),OwnID.getNumber(),WHISPER::SENSOR,ParentPort,ParentIPAddress_,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);
};
@@ -122,7 +121,7 @@ namespace Entities {
WHISPER::Message msg;
incommingGroundThruthMessages->get(msg);
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);
}
}
@@ -156,9 +155,9 @@ namespace Entities {
switch (msg.msgType_) {
case WHISPER::MsgType::OWN_TRACK :{
auto OwnTrack = SimCore::GroundTruthTrack::unpack(msg);
auto OwnTrack = SimCore::SimTrack::unpack(msg);
// SimCore::Track OwnTrack(msg.serialize());
auto tmpPos = OwnTrack.getPostion().getGeocentricPos();
auto tmpPos = OwnTrack.getPosition().getGeocentricPos();
if (this->ownShipPosition_ == nullptr) {
this->ownShipPosition_ = std::make_shared<SimCore::Position>(
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 <Entities/Tracklist/TracklistItem.hpp>
@@ -9,7 +9,7 @@
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);
@@ -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);
@@ -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();
range_ = radarTrack->getRange();
bearing_ = radarTrack->getBearing();
course_ = radarTrack->getCourse();
speed_ = radarTrack->getSpeed();
position_ = track->getPosition();
course_ = track->Course.getValue();
speed_ = track->Speed.getValue();
lastUpdateTimestamp_ = std::chrono::steady_clock::now();
}
if (isSensorinSensorlist(sensorData) != true) {
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();
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();
range_ = radarTrack->getRange();
bearing_ = radarTrack->getBearing();
course_ = radarTrack->getCourse();
speed_ = radarTrack->getSpeed();
position_ = track->getPosition();
course_ = track->Course.getValue();
speed_ = track->Speed.getValue();
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>
@@ -34,11 +35,11 @@ namespace TrackList
{
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();
@@ -76,7 +77,7 @@ namespace TrackList
/// @brief
/// @param track
void TrackList::addTrack(std::shared_ptr<SimCore::Track> track)
void TrackList::addTrack(std::shared_ptr<SimCore::SimTrack> track)
{
auto AllIDs = getAllIDs();
@@ -116,7 +117,7 @@ namespace TrackList
// 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_);
@@ -126,7 +127,7 @@ namespace TrackList
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_);