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

ADD: use of new sensor messages

Browse Source
This commit is contained in:
Henry Winkel
2024-03-07 16:09:34 +01:00
parent bf263b64f6
commit 92cddbbff4
10 changed files with 104 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
.vscode/launch.json vendored Normal file
View File

@@ -0,0 +1,16 @@
{
// Use IntelliSense to learn about possible attributes.
// Hover to view descriptions of existing attributes.
// For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "Debug-SensorManager",
"type": "gdb",
"request": "launch",
"target": "./test_SensorManager",
"cwd": "${workspaceRoot}/build",
"valuesFormatting": "parseText"
}
]
}

8
include/Entities/Sensor.hpp
View File

@@ -1,6 +1,8 @@
#pragma once
#include "DirectCommunicationClient.hpp"
#include "SimCore/Messages/SensorData.hpp"
#include "SimCore/Messages/SensorTrack.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "WHISPER/InternalUDPListener.hpp"
#include <WHISPER/InternalUDPService.hpp>
@@ -28,6 +30,7 @@ namespace Entities {
Sensor(
SimCore::Identifier OwnID,
SimCore::Identifier OwnShipID,
std::string Name,
SimCore::SensorKinds SensorKind,
std::string GroundTruthAddress,
std::uint32_t GroundTruthPort,
@@ -39,19 +42,18 @@ namespace Entities {
protected:
SimCore::SensorData OwnData;
virtual void specificSensorCalculations(std::unique_ptr<SimCore::SimTrack> track) = 0;
std::shared_ptr<SimCore::SimTrack> OwnShipTrack_ = nullptr;
const SimCore::Identifier OwnID_;
const SimCore::Identifier OwnShipID;
SimCore::SensorKinds SensorKind_;
std::shared_ptr<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>> recognisedTracks_ = nullptr;
std::shared_ptr<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SensorTrack>>> recognisedTracks_ = nullptr;
private:
std::uint32_t GroundTruthPort_;

4
include/Entities/SensorManager.hpp
View File

@@ -88,7 +88,7 @@ namespace Entities
*@param string uuid for the Sensor
*@param SimTrack updated Track
*/
void updateTracklistForSensor(std::string uuidSensor, std::shared_ptr<SimCore::SimTrack> track);
void updateTracklistForSensor(std::string uuidSensor, std::shared_ptr<SimCore::SensorTrack> track);
/**
* @brief sends a message to a specific sensor if uuid is passed else it sends to everyone
@@ -103,7 +103,7 @@ namespace Entities
* @param sensor
*
*/
void addSensorLocal(std::unique_ptr<Sensor::SensorControl> sensor);
void addSensorLocal(std::shared_ptr<Sensor::SensorControl> sensor);
/**
* @brief Get the number of sensors

3
include/Entities/Tracklist/Trackfusion.hpp
View File

@@ -3,6 +3,7 @@
#include <SimCore/Messages/TracklistItem.hpp>
#include "SimCore/Identifier.hpp"
#include "SimCore/Messages/SensorTrack.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include <SimCore/Messages/TracklistUpdate.hpp>
#include <map>
@@ -26,7 +27,7 @@ namespace TrackList
* @param track shared pointer of a simtrack
* @param sensorData
*/
void addOrUpdateTrack(std::shared_ptr<SimCore::SimTrack> track,std::shared_ptr<SimCore::SensorData> sensorData);
void addOrUpdateTrack(std::shared_ptr<SimCore::SensorTrack> track);
/**
* @brief deltets a track based on a SimCore::Identifier

13
src/Entities/Sensor.cpp
View File

@@ -1,6 +1,7 @@
#include <SimCore/Position.hpp>
#include "DirectCommunicationClient.hpp"
#include "Orders/Order.hpp"
#include "SimCore/Messages/SensorTrack.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/SimCore.hpp"
#include "SimCore/UtilFunctions.hpp"
@@ -17,22 +18,22 @@ namespace Entities {
Sensor::Sensor(
SimCore::Identifier OwnID,
SimCore::Identifier OwnShipID,
std::string Name,
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)
ParentIPAddress_(ParentIPAddress),
OwnData(OwnID,Name,SimCore::UtilFunctions::getOwnIP(),SensorKind)
{
recognisedTracks_ = std::make_shared<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SimTrack>>>();
recognisedTracks_ = std::make_shared<WHISPER::threadSafeQueue<std::shared_ptr<SimCore::SensorTrack>>>();
GroundTruthUDPListener_ = std::make_unique<WHISPER::InternalUDPListener>(GroundTruthAddr_,GroundTruthPort_);
@@ -41,7 +42,7 @@ namespace Entities {
GroundTruthUDPListener_->connect();
GroundTruthUDPListener_->subscribe(WHISPER::MsgTopics::TRACK);
client_ = std::make_unique<DirectCommunication::DirectCommunicationClient>(ParentPort_,ParentIPAddress_,this->OwnID_.getUUID());
client_ = std::make_unique<DirectCommunication::DirectCommunicationClient>(ParentPort_,ParentIPAddress_,this->OwnData.getID().getUUID());
client_->registerMessageCallback(std::bind(&Sensor::handlServerMessages,this,std::placeholders::_1));
client_->sendMessage("Hello Server");
@@ -192,7 +193,7 @@ void Sensor::updateOwnShipFunction()
{
if (recognisedTracks_->size() > 0)
{
std::shared_ptr<SimCore::SimTrack> track;
std::shared_ptr<SimCore::SensorTrack> track;
recognisedTracks_->get(track);
if (track != nullptr)
{

12
src/Entities/SensorManager.cpp
View File

@@ -1,5 +1,6 @@
#include "Entities/SensorControl.hpp"
#include "SimCore/Identifier.hpp"
#include "SimCore/Messages/SensorTrack.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/SimCore.hpp"
#include "WHISPER/Messages/Message.hpp"
@@ -80,7 +81,7 @@ namespace Entities
}
void SensorManager::addSensorLocal(std::unique_ptr<Sensor::SensorControl> sensor)
void SensorManager::addSensorLocal(std::shared_ptr<Sensor::SensorControl> sensor)
{
SensorStore[sensor->getID().getUUID()] = std::move(sensor);
}
@@ -101,14 +102,15 @@ namespace Entities
switch (msg.msgType_)
{
case WHISPER::MsgType::SIM_TRACK:
case WHISPER::MsgType::SENSOR_TRACK:
{
auto track = std::make_shared<SimCore::SimTrack>(SimCore::SimTrack::unpack(msg));
auto test = SimCore::SensorTrack::unpack(msg);
std::shared_ptr<SimCore::SensorTrack> track = std::move(test);
if(track != nullptr)
{
updateTracklistForSensor(msg.senderUUID_, track);
auto sensor = getSensorByUUID(msg.senderUUID_);
Trackfusion_.addOrUpdateTrack(track, sensor);
Trackfusion_.addOrUpdateTrack(track);
}
break;
}
@@ -124,7 +126,7 @@ namespace Entities
void SensorManager::updateTracklistForSensor(std::string uuidSensor, std::shared_ptr<SimCore::SimTrack> track)
void SensorManager::updateTracklistForSensor(std::string uuidSensor, std::shared_ptr<SimCore::SensorTrack> track)
{
auto sensor= getSensorByUUID(uuidSensor);

17
src/Entities/Tracklist/Trackfusion.cpp
View File

@@ -1,4 +1,5 @@
#include <SimCore/Messages/TracklistItem.hpp>
#include "SimCore/Messages/SensorTrack.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/Messages/TracklistUpdate.hpp"
#include "nlohmann/json_fwd.hpp"
@@ -10,20 +11,28 @@
namespace TrackList
{
void Trackfusion::addOrUpdateTrack(std::shared_ptr<SimCore::SimTrack> track, std::shared_ptr<SimCore::SensorData> sensorData)
void Trackfusion::addOrUpdateTrack(std::shared_ptr<SimCore::SensorTrack> track)
{
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);
std::shared_ptr<SimCore::TracklistItem> Item;
if (track->getSensorData() == nullptr) {
Item = std::make_shared<SimCore::TracklistItem>(track);
}else {
Item = std::make_shared<SimCore::TracklistItem>(track,*track->getSensorData().get());
// Item->addSensorDataToSensorList();
}
TrackStore_[Item->getIdentifier()] = Item;
}else {
iterator->second->updateTrack(track);
iterator->second->addSensorDataToSensorList(*sensorData);
if (track->getSensorData() != nullptr) {
iterator->second->addSensorDataToSensorList(*track->getSensorData());
}
}

2
tests/SensorManagerOutput.json Normal file
View File

@@ -0,0 +1,2 @@
[{"SensorType":"Radar","Sensors":[{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"false","SensorID":"4c4e8e93-a935-49ee-a2a2-7cd474bff47f","SensorName":"Sensor","SensorStatus":"Unknown"}]},{"SensorType":"ESMSensor"},{"SensorType":"Sonar"},{"SensorType":"VisualSensor"},{"SensorType":"Radar","Sensors":[{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"false","SensorID":"4c4e8e93-a935-49ee-a2a2-7cd474bff47f","SensorName":"Sensor","SensorStatus":"Unknown"}]},{"SensorType":"ESMSensor"},{"SensorType":"Sonar"},{"SensorType":"VisualSensor","Sensors":[{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"true","SensorID":"bce82b25-5796-4c45-89ca-524d599dfa12","SensorName":"Sensor2","SensorStatus":"Active"}]},{"SensorType":"Radar","Sensors":[{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"false","SensorID":"4c4e8e93-a935-49ee-a2a2-7cd474bff47f","SensorName":"Sensor","SensorStatus":"Unknown"},{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"false","SensorID":"c7f97190-0f71-477e-9a03-382089c71f4d","SensorName":"Sensor1","SensorStatus":"Unknown"}]},{"SensorType":"ESMSensor"},{"SensorType":"Sonar"},{"SensorType":"VisualSensor","Sensors":[{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"true","SensorID":"bce82b25-5796-4c45-89ca-524d599dfa12","SensorName":"Sensor2","SensorStatus":"Active"}]},{"SensorType":"Radar","Sensors":[{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"false","SensorID":"4c4e8e93-a935-49ee-a2a2-7cd474bff47f","SensorName":"Sensor","SensorStatus":"Unknown"},{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"false","SensorID":"c7f97190-0f71-477e-9a03-382089c71f4d","SensorName":"Sensor1","SensorStatus":"Unknown"}]},{"SensorType":"ESMSensor","Sensors":[{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"false","SensorID":"d2b862ca-a950-40d7-8e62-28523ada4093","SensorName":"Sensor3","SensorStatus":"Unknown"}]},{"SensorType":"Sonar"},{"SensorType":"VisualSensor","Sensors":[{"SensorContacts":[null],"SensorDamaged":"false","SensorEnabled":"true","SensorID":"bce82b25-5796-4c45-89ca-524d599dfa12","SensorName":"Sensor2","SensorStatus":"Active"}]}]

6
tests/test_SensorClass.cpp
View File

@@ -2,6 +2,7 @@
#include <SimCore/Identifier.hpp>
#include <SimCore/SimCore.hpp>
#include <memory>
#include <string>
#include <thread>
#define CATCH_CONFIG_MAIN
#include <catch2/catch.hpp>
@@ -13,12 +14,13 @@ class Radar : public Entities::Sensor
public:
Radar(SimCore::Identifier OwnID,
SimCore::Identifier ParentID,
std::string Name,
SimCore::SensorKinds SensorKind,
std::string GroundTruthAddress,
std::uint32_t GroundTruthPort,
std::uint32_t ParentPort,
std::string ParentIPAddress,
std::string radarType):Sensor(OwnID, ParentID, SensorKind,GroundTruthAddress, GroundTruthPort, ParentPort, ParentIPAddress),radarType_(radarType)
std::string radarType):Sensor(OwnID, ParentID, Name, SensorKind,GroundTruthAddress, GroundTruthPort, ParentPort, ParentIPAddress),radarType_(radarType)
{
}
@@ -46,7 +48,7 @@ SCENARIO("Testing the SimCore Sensor")
SimCore::Identifier IDParent;
SimCore::Identifier IDRadar;
Radar Radar(IDRadar,IDParent,SimCore::SensorKinds::RADAR,"239.0.0.1",8000,8001,"127.0.0.1","APAR");
Radar Radar(IDRadar,IDParent,"APAR", SimCore::SensorKinds::RADAR,"239.0.0.1",8000,8001,"127.0.0.1","APAR");
Radar.start();
WHEN("constructing Track Object with data")

70
tests/test_SensorManager.cpp
View File

@@ -2,6 +2,8 @@
#include "DirectCommunicationClient.hpp"
#include "Entities/SensorControl.hpp"
#include "Entities/SensorManager.hpp"
#include "SimCore/Messages/SensorData.hpp"
#include "SimCore/Messages/SensorTrack.hpp"
#include "SimCore/Messages/SimTrack.hpp"
#include "SimCore/Position.hpp"
#include "nlohmann/json_fwd.hpp"
@@ -22,18 +24,20 @@ double fRand(double fMin, double fMax)
double f = (double)rand() / RAND_MAX;
return fMin + f * (fMax - fMin);
}
void sendRandomTrack(std::shared_ptr<DirectCommunication::DirectCommunicationClient> client, int number)
void sendRandomTrack(std::shared_ptr<DirectCommunication::DirectCommunicationClient> client,std::shared_ptr<Sensor::SensorControl> sensor, int number)
{
for (int i = 0; i < number; i++) {
std::string name = "test1-" + std::to_string(i);
SimCore::SimTrack track(name,SimCore::Kind::EntityKind::SURFACE,SimCore::Side::NEUTRAL);
auto simtrack = std::make_shared<SimCore::SimTrack>(name,SimCore::Kind::EntityKind::SURFACE,SimCore::Side::NEUTRAL);
auto track = std::make_shared<SimCore::SensorTrack>(simtrack,sensor);
// LOG_S(INFO)<<track->getSensorData()->getName();
SimCore::Position pos;
double lat = fRand(-90, 90);
double lon = fRand(-180, 180);
pos.setGeodesicPos( lat, lon, 0);
track.setPosition(pos);
client->sendMessage(track.buildMessage());
track->setPosition(pos);
client->sendMessage(track->buildMessage());
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
@@ -42,16 +46,16 @@ for (int i = 0; i < number; i++) {
void sendRandomTracktoManager(std::shared_ptr<Entities::SensorManager> SensorManager_)
{
for (int i = 0; i < 5; i++) {
std::string name = "test1-" + std::to_string(i);
SimCore::SimTrack track(name,SimCore::Kind::EntityKind::SURFACE,SimCore::Side::NEUTRAL);
SimCore::Position pos;
double lat = fRand(-90, 90);
double lon = fRand(-180, 180);
// for (int i = 0; i < 5; i++) {
// std::string name = "test1-" + std::to_string(i);
// SimCore::SensorTrack track(name,SimCore::Kind::EntityKind::SURFACE,SimCore::Side::NEUTRAL);
// SimCore::Position pos;
// double lat = fRand(-90, 90);
// double lon = fRand(-180, 180);
pos.setGeodesicPos( lat, lon, 0);
track.setPosition(pos);
}
// pos.setGeodesicPos( lat, lon, 0);
// track.setPosition(pos);
// }
@@ -59,39 +63,48 @@ for (int i = 0; i < 5; i++) {
auto id0 = SimCore::Identifier();
LOG_S(INFO)<<id0.getUUID();
auto control = std::make_unique<Sensor::SensorControl>(id0.getUUID(),"Sensor", "127.0.0.1", SimCore::SensorKinds::RADAR);
auto control = std::make_shared<Sensor::SensorControl>(id0.getUUID(),"Sensor", "127.0.0.1", SimCore::SensorKinds::RADAR);
auto test = std::static_pointer_cast<SimCore::SensorData>(control);
LOG_S(INFO)<< test->getSensorDataAsJson();
auto client = std::make_shared<DirectCommunication::DirectCommunicationClient>(5557,"127.0.0.1",id0.getUUID());
SensorManager_->addSensorLocal(std::move(control));
SensorManager_->addSensorLocal(control);
// sendRandomTrack(client,5);
auto id1 = SimCore::Identifier();
auto control1 = std::make_unique<Sensor::SensorControl>(id1.getUUID(),"Sensor1", "127.0.0.1", SimCore::SensorKinds::RADAR);
auto control1 = std::make_shared<Sensor::SensorControl>(id1.getUUID(),"Sensor1", "127.0.0.1", SimCore::SensorKinds::RADAR);
auto client1 = std::make_shared<DirectCommunication::DirectCommunicationClient>(5557,"127.0.0.1",id1.getUUID());
SensorManager_->addSensorLocal(std::move(control1));
SensorManager_->addSensorLocal(control1);
// sendRandomTrack(client1,5);
auto id2 = SimCore::Identifier();
auto control2 = std::make_unique<Sensor::SensorControl>(id2.getUUID(),"Sensor2", "127.0.0.1", SimCore::SensorKinds::VISUAL);
auto control2 = std::make_shared<Sensor::SensorControl>(id2.getUUID(),"Sensor2", "127.0.0.1", SimCore::SensorKinds::VISUAL);
auto client2 = std::make_shared<DirectCommunication::DirectCommunicationClient>(5557,"127.0.0.1",id2.getUUID());
SensorManager_->addSensorLocal(std::move(control2));
SensorManager_->addSensorLocal(control2);
// sendRandomTrack(client2,5);
auto id3 = SimCore::Identifier();
auto control3 = std::make_unique<Sensor::SensorControl>(id3.getUUID(),"Sensor3", "127.0.0.1", SimCore::SensorKinds::ESM);
auto control3 = std::make_shared<Sensor::SensorControl>(id3.getUUID(),"Sensor3", "127.0.0.1", SimCore::SensorKinds::ESM);
auto client3 = std::make_shared<DirectCommunication::DirectCommunicationClient>(5557,"127.0.0.1",id3.getUUID());
SensorManager_->addSensorLocal(std::move(control3));
sendRandomTrack(client2,5);
SensorManager_->addSensorLocal(control3);
sendRandomTrack(client2,control3,5);
for (int i = 0; i < 3; i++) {
std::string name = "identical-" + std::to_string(i);
SimCore::SimTrack track(name,SimCore::Kind::EntityKind::SURFACE,SimCore::Side::NEUTRAL);
auto Simtrack = std::make_shared<SimCore::SimTrack>(name,SimCore::Kind::EntityKind::SURFACE,SimCore::Side::NEUTRAL);
SimCore::SensorTrack track(Simtrack);
// LOG_S(INFO)<<track.getSensorData()->getID().getUUID();
SimCore::Position pos;
double lat = fRand(-90, 90);
double lon = fRand(-180, 180);
pos.setGeodesicPos( lat, lon, 0);
track.setPosition(pos);
client->sendMessage(track.buildMessage());
std::this_thread::sleep_for(std::chrono::milliseconds(100));
client1->sendMessage(track.buildMessage());
@@ -141,6 +154,7 @@ void startSensors(SimCore::Identifier ownid, std::string GroundTruthAddr_,ushort
SensorManager_->startSensor(RadarPod, SimCore::SensorKinds::RADAR);
}
SCENARIO("Testing the SimCore SensorManager")
@@ -151,8 +165,8 @@ SCENARIO("Testing the SimCore SensorManager")
auto PodController_ = std::make_shared<kubecontrol::PodController>("docs/config");
auto SensorManager_ = std::make_shared<Entities::SensorManager>(id, PodController_,5557);
auto ownShip = std::make_shared<SimCore::SimTrack>(id,"Test Ship", SimCore::Kind::EntityKind::SURFACE,SimCore::Side::EntitySide::FRIEND);
auto SensorManager_ = std::make_shared<Entities::SensorManager>(ownShip, PodController_,5557);
startSensors(id, "239.0.0.1", 7000, 5557, SensorManager_);
@@ -172,7 +186,7 @@ SCENARIO("Testing the SimCore SensorManager")
REQUIRE(PodController_->getInfoForAllPods().size() == 0);
SensorManager_->stop();
} //THEN
} // WHEN
} // GIVEN
@@ -187,8 +201,9 @@ SCENARIO("Testing the SimCore SensorManager with local sensors")
auto PodController_ = std::make_shared<kubecontrol::PodController>("docs/config");
auto ownShip = std::make_shared<SimCore::SimTrack>(id,"Test Ship", SimCore::Kind::EntityKind::SURFACE,SimCore::Side::EntitySide::FRIEND);
auto SensorManager_ = std::make_shared<Entities::SensorManager>(id, PodController_,5557);
auto SensorManager_ = std::make_shared<Entities::SensorManager>(ownShip, PodController_,5557);
sendRandomTracktoManager(SensorManager_);
@@ -222,6 +237,7 @@ SCENARIO("Testing the SimCore SensorManager with local sensors")
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
SensorManager_->stop();
} //THEN
} // WHEN