From 92cddbbff4e6aa57e15dced6fc94685add8d13db Mon Sep 17 00:00:00 2001 From: Henry Winkel Date: Thu, 7 Mar 2024 16:09:34 +0100 Subject: [PATCH] ADD: use of new sensor messages --- .vscode/launch.json | 16 +++++ include/Entities/Sensor.hpp | 8 ++- include/Entities/SensorManager.hpp | 4 +- include/Entities/Tracklist/Trackfusion.hpp | 3 +- src/Entities/Sensor.cpp | 15 ++--- src/Entities/SensorManager.cpp | 12 ++-- src/Entities/Tracklist/Trackfusion.cpp | 19 ++++-- tests/SensorManagerOutput.json | 2 + tests/test_SensorClass.cpp | 6 +- tests/test_SensorManager.cpp | 72 +++++++++++++--------- 10 files changed, 104 insertions(+), 53 deletions(-) create mode 100644 .vscode/launch.json create mode 100644 tests/SensorManagerOutput.json diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..5f3d4a1 --- /dev/null +++ b/.vscode/launch.json @@ -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" + } + ] +} \ No newline at end of file diff --git a/include/Entities/Sensor.hpp b/include/Entities/Sensor.hpp index 4bb996a..d2d4dfe 100644 --- a/include/Entities/Sensor.hpp +++ b/include/Entities/Sensor.hpp @@ -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 @@ -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 track) = 0; std::shared_ptr OwnShipTrack_ = nullptr; - const SimCore::Identifier OwnID_; const SimCore::Identifier OwnShipID; - SimCore::SensorKinds SensorKind_; - std::shared_ptr>> recognisedTracks_ = nullptr; + std::shared_ptr>> recognisedTracks_ = nullptr; private: std::uint32_t GroundTruthPort_; diff --git a/include/Entities/SensorManager.hpp b/include/Entities/SensorManager.hpp index 5064098..b1d2b3b 100644 --- a/include/Entities/SensorManager.hpp +++ b/include/Entities/SensorManager.hpp @@ -88,7 +88,7 @@ namespace Entities *@param string uuid for the Sensor *@param SimTrack updated Track */ - void updateTracklistForSensor(std::string uuidSensor, std::shared_ptr track); + void updateTracklistForSensor(std::string uuidSensor, std::shared_ptr 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); + void addSensorLocal(std::shared_ptr sensor); /** * @brief Get the number of sensors diff --git a/include/Entities/Tracklist/Trackfusion.hpp b/include/Entities/Tracklist/Trackfusion.hpp index 3e081d6..83901c3 100644 --- a/include/Entities/Tracklist/Trackfusion.hpp +++ b/include/Entities/Tracklist/Trackfusion.hpp @@ -3,6 +3,7 @@ #include #include "SimCore/Identifier.hpp" +#include "SimCore/Messages/SensorTrack.hpp" #include "SimCore/Messages/SimTrack.hpp" #include #include @@ -26,7 +27,7 @@ namespace TrackList * @param track shared pointer of a simtrack * @param sensorData */ - void addOrUpdateTrack(std::shared_ptr track,std::shared_ptr sensorData); + void addOrUpdateTrack(std::shared_ptr track); /** * @brief deltets a track based on a SimCore::Identifier diff --git a/src/Entities/Sensor.cpp b/src/Entities/Sensor.cpp index cbeb43d..87f1878 100644 --- a/src/Entities/Sensor.cpp +++ b/src/Entities/Sensor.cpp @@ -1,6 +1,7 @@ #include #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>>(); + + recognisedTracks_ = std::make_shared>>(); GroundTruthUDPListener_ = std::make_unique(GroundTruthAddr_,GroundTruthPort_); @@ -41,7 +42,7 @@ namespace Entities { GroundTruthUDPListener_->connect(); GroundTruthUDPListener_->subscribe(WHISPER::MsgTopics::TRACK); - client_ = std::make_unique(ParentPort_,ParentIPAddress_,this->OwnID_.getUUID()); + client_ = std::make_unique(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 track; + std::shared_ptr track; recognisedTracks_->get(track); if (track != nullptr) { diff --git a/src/Entities/SensorManager.cpp b/src/Entities/SensorManager.cpp index f9c33a9..89dd149 100644 --- a/src/Entities/SensorManager.cpp +++ b/src/Entities/SensorManager.cpp @@ -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) + void SensorManager::addSensorLocal(std::shared_ptr 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::unpack(msg)); + auto test = SimCore::SensorTrack::unpack(msg); + std::shared_ptr 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 track) + void SensorManager::updateTracklistForSensor(std::string uuidSensor, std::shared_ptr track) { auto sensor= getSensorByUUID(uuidSensor); diff --git a/src/Entities/Tracklist/Trackfusion.cpp b/src/Entities/Tracklist/Trackfusion.cpp index cadadc2..8db414a 100644 --- a/src/Entities/Tracklist/Trackfusion.cpp +++ b/src/Entities/Tracklist/Trackfusion.cpp @@ -1,4 +1,5 @@ #include +#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 track, std::shared_ptr sensorData) + void Trackfusion::addOrUpdateTrack(std::shared_ptr track) { std::lock_guard lock(mx_); - auto iterator = TrackStore_.find(track->getIdentifier()); if (iterator == TrackStore_.end()) - { - auto Item = std::make_shared(track,*sensorData); + { + std::shared_ptr Item; + if (track->getSensorData() == nullptr) { + + Item = std::make_shared(track); + }else { + Item = std::make_shared(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()); + } } diff --git a/tests/SensorManagerOutput.json b/tests/SensorManagerOutput.json new file mode 100644 index 0000000..12ce231 --- /dev/null +++ b/tests/SensorManagerOutput.json @@ -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"}]}] \ No newline at end of file diff --git a/tests/test_SensorClass.cpp b/tests/test_SensorClass.cpp index 21c432e..12b7e2d 100644 --- a/tests/test_SensorClass.cpp +++ b/tests/test_SensorClass.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include #define CATCH_CONFIG_MAIN #include @@ -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") diff --git a/tests/test_SensorManager.cpp b/tests/test_SensorManager.cpp index b527f8f..1a0e080 100644 --- a/tests/test_SensorManager.cpp +++ b/tests/test_SensorManager.cpp @@ -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 client, int number) +void sendRandomTrack(std::shared_ptr client,std::shared_ptr 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(name,SimCore::Kind::EntityKind::SURFACE,SimCore::Side::NEUTRAL); + auto track = std::make_shared(simtrack,sensor); + // LOG_S(INFO)<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 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(),"Sensor", "127.0.0.1", SimCore::SensorKinds::RADAR); + auto control = std::make_shared(id0.getUUID(),"Sensor", "127.0.0.1", SimCore::SensorKinds::RADAR); + auto test = std::static_pointer_cast(control); + LOG_S(INFO)<< test->getSensorDataAsJson(); + auto client = std::make_shared(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(id1.getUUID(),"Sensor1", "127.0.0.1", SimCore::SensorKinds::RADAR); + auto control1 = std::make_shared(id1.getUUID(),"Sensor1", "127.0.0.1", SimCore::SensorKinds::RADAR); auto client1 = std::make_shared(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(id2.getUUID(),"Sensor2", "127.0.0.1", SimCore::SensorKinds::VISUAL); + auto control2 = std::make_shared(id2.getUUID(),"Sensor2", "127.0.0.1", SimCore::SensorKinds::VISUAL); auto client2 = std::make_shared(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(id3.getUUID(),"Sensor3", "127.0.0.1", SimCore::SensorKinds::ESM); + auto control3 = std::make_shared(id3.getUUID(),"Sensor3", "127.0.0.1", SimCore::SensorKinds::ESM); auto client3 = std::make_shared(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(name,SimCore::Kind::EntityKind::SURFACE,SimCore::Side::NEUTRAL); + SimCore::SensorTrack track(Simtrack); + + // LOG_S(INFO)<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()); @@ -140,6 +153,7 @@ void startSensors(SimCore::Identifier ownid, std::string GroundTruthAddr_,ushort RadarPod->setComponent("Radar"); SensorManager_->startSensor(RadarPod, SimCore::SensorKinds::RADAR); + } @@ -151,14 +165,14 @@ SCENARIO("Testing the SimCore SensorManager") auto PodController_ = std::make_shared("docs/config"); - - auto SensorManager_ = std::make_shared(id, PodController_,5557); + auto ownShip = std::make_shared(id,"Test Ship", SimCore::Kind::EntityKind::SURFACE,SimCore::Side::EntitySide::FRIEND); + auto SensorManager_ = std::make_shared(ownShip, PodController_,5557); startSensors(id, "239.0.0.1", 7000, 5557, SensorManager_); - + WHEN("constructing Track Object with data") { @@ -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("docs/config"); + auto ownShip = std::make_shared(id,"Test Ship", SimCore::Kind::EntityKind::SURFACE,SimCore::Side::EntitySide::FRIEND); - auto SensorManager_ = std::make_shared(id, PodController_,5557); + auto SensorManager_ = std::make_shared(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