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

ADD: made position Threadsafe, and made it possible to update the sensor position

Browse Source
This commit is contained in:
Henry Winkel
2023-01-20 12:35:21 +01:00
parent 63c9757ec3
commit 982b701032
7 changed files with 106 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
include/SimCore/Messages/Track.hpp
View File

@@ -15,6 +15,7 @@
#include <memory>
#include <string>
#include <loguru.hpp>
#include <type_traits>
namespace SimCore {
@@ -54,7 +55,6 @@ namespace SimCore {
/**
* @brief constructor for building a track which is also the trackmessage to send
* @param uint32_t deviceID the id of the sending device
* @param WHISPER::SourceType sourcetype of the sending device
* @param SimCore::Identifier object identifier
*
@@ -62,6 +62,15 @@ namespace SimCore {
*/
Track( WHISPER::SourceType src,SimCore::Identifier id);
/**
* @brief constructor for building a track which is also the trackmessage to send
* @param WHISPER::SourceType sourcetype of the sending device
* @param SimCore::Identifier object identifier
* @param WHISPER::MsgType other message type than RawTrack
*
* @return
*/
Track( WHISPER::SourceType src,SimCore::Identifier id, WHISPER::MsgType type);
/**

9
include/SimCore/Position.hpp
View File

@@ -1,5 +1,6 @@
#pragma once
#include "SimCore/SimCore.hpp"
#include <Eigen/Core>
#include <GeographicLib/Ellipsoid.hpp>
#include <GeographicLib/Geocentric.hpp>
@@ -7,6 +8,7 @@
#include <GeographicLib/GeodesicLine.hpp>
#include <GeographicLib/Constants.hpp>
#include <memory>
#include <mutex>
namespace SimCore {
class Position {
@@ -14,6 +16,8 @@ namespace SimCore {
Position();
Position(double X, double Y, double Z);
Position(const Position& other);
/**
* @brief returns a eigen vector3d with the X, Y, Z coordinates
* @return Eigen::Vector3d
@@ -31,6 +35,9 @@ namespace SimCore {
bool operator== ( Position &lhs);
Position& operator=(const Position other);
private:
std::shared_ptr<GeographicLib::Geocentric> earth_ = nullptr;
@@ -40,6 +47,8 @@ namespace SimCore {
Eigen::Vector3d GeodesicPos_; // lat long height
Eigen::Vector3d GeocentricPos_; ///x y z
mutable std::mutex mx;

7
include/SimCore/Sensor.hpp
View File

@@ -36,6 +36,8 @@ namespace SimCore {
virtual void specificSensorCalculations() = 0;
virtual void specificReloadCharacteristicts() = 0;
std::shared_ptr<Position> ownShipPosition_ = nullptr;
private:
SimCore::Identifier OwnID_;
@@ -48,8 +50,8 @@ namespace SimCore {
std::shared_ptr<WHISPER::InternalUDPService> GroundTruthUDPService_ = nullptr;
std::shared_ptr<WHISPER::InternalUDPService> ParentUDPService_ = nullptr;
void receivingData();
void sendingData();
void groundThruthData();
void parentData();
void SensorCalculations();
void ReloadCharacteristicts();
@@ -69,7 +71,6 @@ namespace SimCore {
std::thread sensorCalculationThread;
std::shared_ptr<Position> position_ = nullptr;
};

13
src/SimCore/Messages/Track.cpp
View File

@@ -15,7 +15,8 @@
namespace SimCore {
Track::Track(std::string receivedMessage){
Track::Track(std::string receivedMessage)
{
msg = messages::header::Message();
try {
msg.ParseFromString(receivedMessage);
@@ -30,13 +31,10 @@ namespace SimCore {
}
}
// messages::track::Identifier ID = trackMessage.entityidentifier();
ID_ = SimCore::Identifier(trackMessage.mutable_entityidentifier()->parent(),
trackMessage.mutable_entityidentifier()->number(),
(SimCore::ObjectSource)trackMessage.mutable_entityidentifier()->external());
external_ = trackMessage.mutable_entityidentifier()->external();
// trackNo_ = trackMessage.trackno();
// external_ = trackMessage.external();
speed_ = trackMessage.speed();
course_ = trackMessage.course();
@@ -60,6 +58,13 @@ namespace SimCore {
}
Track::Track( WHISPER::SourceType src,SimCore::Identifier id, WHISPER::MsgType type):
Message(id.getParentNumber(),id.getNumber(),WHISPER::MsgTopics::TRACK,type,src),external_(id.isExternal()),ID_(id)
{
packToMessage();
}
void Track::packToMessage()
{

32
src/SimCore/Position.cpp
View File

@@ -4,14 +4,14 @@
namespace SimCore {
Position::Position()
Position::Position():mx()
{
earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
wgs84_ = std::make_unique<GeographicLib::Ellipsoid>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
}
Position::Position(double X, double Y, double Z)
Position::Position(double X, double Y, double Z):mx()
{
earth_ = std::make_unique<GeographicLib::Geocentric>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
geod_ = std::make_unique<GeographicLib::Geodesic>(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
@@ -19,18 +19,32 @@ namespace SimCore {
setGeocentricPos( X, Y, Z);
}
Position::Position(const Position& other){
std::lock_guard<std::mutex> lock(other.mx);
GeocentricPos_ = other.GeocentricPos_;
GeodesicPos_ = other.GeodesicPos_;
}
Eigen::Vector3d Position::getGeocentricPos()
{
std::lock_guard<std::mutex> lock(mx);
return GeocentricPos_;
}
Eigen::Vector3d Position::getGeodesicPos()
{
std::lock_guard<std::mutex> lock(mx);
return GeodesicPos_;
}
void Position::setGeocentricPos(double X, double Y, double Z){
std::lock_guard<std::mutex> lock(mx);
GeocentricPos_(GeocentricPosition::X) = X;
GeocentricPos_(GeocentricPosition::Y) = Y;
GeocentricPos_(GeocentricPosition::Z)= Z;
@@ -39,6 +53,8 @@ namespace SimCore {
}
void Position::setGeodesicPos(double lat, double lon, int height){
std::lock_guard<std::mutex> lock(mx);
GeodesicPos_(GeodesicPosition::LATITUDE) = lat;
GeodesicPos_(GeodesicPosition::LONGITUDE) = lon;
GeodesicPos_(GeodesicPosition::HEIGHT) = height;
@@ -47,6 +63,8 @@ namespace SimCore {
bool Position::operator==( Position &lhs)
{
std::lock_guard<std::mutex> lock(mx);
if (this->GeocentricPos_ == lhs.getGeocentricPos()) {
return true;
}else{
@@ -55,6 +73,16 @@ namespace SimCore {
}
Position& Position::operator=(const Position other)
{
std::lock_guard<std::mutex> lock(other.mx);
GeocentricPos_ = other.GeocentricPos_;
GeodesicPos_ = other.GeodesicPos_;
return *this;
}
}

34
src/SimCore/Sensor.cpp
View File

@@ -1,4 +1,6 @@
#include "SimCore/Messages/Track.hpp"
#include "SimCore/Position.hpp"
#include "SimCore/SimCore.hpp"
#include "SimCore/UtilFunctions.hpp"
#include "WHISPER/Messages/Message.hpp"
#include <SimCore/Sensor.hpp>
@@ -66,10 +68,10 @@ namespace SimCore {
stopReceivingGroundThruth = false;
receiveGroundTruthThread = std::thread(&Sensor::receivingData,this);
receiveGroundTruthThread = std::thread(&Sensor::groundThruthData,this);
stopsendCalculatedData = false;
sendCalculatedDataThread = std::thread(&Sensor::sendingData,this);
sendCalculatedDataThread = std::thread(&Sensor::parentData,this);
stopCalculationData = false;
sensorCalculationThread = std::thread(&Sensor::SensorCalculations,this);
@@ -107,7 +109,7 @@ namespace SimCore {
}
void Sensor::receivingData()
void Sensor::groundThruthData()
{
this->ReceivingGroundThruthIsRunnung = true;
GroundTruthUDPService_->connect(incommingGroundThruthMessages);
@@ -134,7 +136,7 @@ namespace SimCore {
this->ReceivingGroundThruthIsRunnung = false;
}
void Sensor::sendingData()
void Sensor::parentData()
{
this->sendCalculatedDataIsRunnung = true;
@@ -145,9 +147,31 @@ namespace SimCore {
if (incommingParentMessages->size() > 0) {
WHISPER::Message msg;
incommingGroundThruthMessages->get(msg);
incommingParentMessages->get(msg);
LOG_S(INFO)<< "Message received from Parent is" << msg.msgType_;
std::uint32_t type = 0;
if (msg.msgType_ == WHISPER::MsgType::OWN_TRACK) {
SimCore::Track OwnTrack(msg.serialize());
auto tmpPos = OwnTrack.getPostion().getGeocentricPos();
if (this->ownShipPosition_ == nullptr) {
this->ownShipPosition_ = std::make_shared<SimCore::Position>(
tmpPos[SimCore::GeocentricPosition::X],
tmpPos[SimCore::GeocentricPosition::Y],
tmpPos[SimCore::GeocentricPosition::Z]);
}else {
this->ownShipPosition_->setGeocentricPos(
tmpPos[SimCore::GeocentricPosition::X],
tmpPos[SimCore::GeocentricPosition::Y],
tmpPos[SimCore::GeocentricPosition::Z]);
}
}
}
if (outgoingParentMessages->size() > 0) {
WHISPER::Message msg;

9
tests/test_PositionClass.cpp
View File

@@ -24,9 +24,11 @@ SCENARIO("Testing the SimCorePositionClass")
WHEN("constructing Position Object with data")
{
SimCore::Position pos1( GeocentPos1(SimCore::GeocentricPosition::X), GeocentPos1(SimCore::GeocentricPosition::Y), GeocentPos1(SimCore::GeocentricPosition::Z));
SimCore::Position pos1b;
SimCore::Position pos1b, pos2;
pos1b.setGeodesicPos(GeodesPos1(SimCore::GeodesicPosition::LATITUDE), GeodesPos1(SimCore::GeodesicPosition::LONGITUDE), GeodesPos1(SimCore::GeodesicPosition::HEIGHT));
pos2 = pos1;
THEN("positions attributes are correct")
{
REQUIRE(pos1.getGeocentricPos()(SimCore::GeocentricPosition::X) == GeocentPos1(SimCore::GeocentricPosition::X));
@@ -38,6 +40,11 @@ SCENARIO("Testing the SimCorePositionClass")
REQUIRE(std::abs(pos1b.getGeocentricPos()(SimCore::Y) - GeocentPos1(SimCore::Y)) <= 0.001 );
REQUIRE(std::abs(pos1b.getGeocentricPos()(SimCore::Z) - GeocentPos1(SimCore::Z)) <= 0.001);
REQUIRE(pos2.getGeocentricPos() == pos1.getGeocentricPos());
REQUIRE(pos2.getGeodesicPos() == pos1.getGeodesicPos());
} //THEN
} // WHEN
} // GIVEN