From 10f92fb9667a135d2b8b57c6142a847e760bae88 Mon Sep 17 00:00:00 2001 From: hwinkel Date: Sat, 27 May 2023 21:34:53 +0200 Subject: [PATCH] CHG: movement now uses sharedptr for the position --- include/Entities/Movement.hpp | 12 ++++----- src/Entities/Entity.cpp | 6 ++--- src/Entities/Movement.cpp | 47 ++++++++++++++++------------------- 3 files changed, 31 insertions(+), 34 deletions(-) diff --git a/include/Entities/Movement.hpp b/include/Entities/Movement.hpp index 01d1c5d..2252db2 100644 --- a/include/Entities/Movement.hpp +++ b/include/Entities/Movement.hpp @@ -25,10 +25,10 @@ namespace Entities { public: Movement(); - Movement(SimCore::Position pos); - Movement(SimCore::Position pos, double course); - Movement(SimCore::Position pos, double course, double speed); - Movement(SimCore::Position pos, double course, double speed, double pitch); + Movement(std::shared_ptr pos); + Movement(std::shared_ptr, double course); + Movement(std::shared_ptr, double course, double speed); + Movement(std::shared_ptr, double course, double speed, double pitch); @@ -69,7 +69,7 @@ namespace Entities */ Eigen::Vector3d getKinematicVector(); - SimCore::Position getPosition(); + std::shared_ptr getPosition(); void setPosition(double lat, double lon, double height = 0); @@ -85,7 +85,7 @@ namespace Entities private: - SimCore::Position ownPosition_; + std::shared_ptr ownPosition_; SimCore::Orientation ownOrientation_; Eigen::Vector3d kinematicVec_; diff --git a/src/Entities/Entity.cpp b/src/Entities/Entity.cpp index 2e1ee92..9b6afce 100644 --- a/src/Entities/Entity.cpp +++ b/src/Entities/Entity.cpp @@ -155,7 +155,7 @@ namespace Entities } - LOG_S(INFO)<<"hello from command worker"; + // LOG_S(INFO)<<"hello from command worker"; std::this_thread::sleep_for(std::chrono::milliseconds(900)); } @@ -169,7 +169,7 @@ namespace Entities while (!stopSensorWorker) { - LOG_S(INFO)<<"hello from sensor worker"; + // LOG_S(INFO)<<"hello from sensor worker"; std::this_thread::sleep_for(std::chrono::milliseconds(900)); } @@ -180,7 +180,7 @@ namespace Entities { while (!stopTrackWorker) { - LOG_S(INFO)<<"hello from track worker"; + // LOG_S(INFO)<<"hello from track worker"; std::this_thread::sleep_for(std::chrono::milliseconds(900)); } diff --git a/src/Entities/Movement.cpp b/src/Entities/Movement.cpp index 688d4d2..36a0361 100644 --- a/src/Entities/Movement.cpp +++ b/src/Entities/Movement.cpp @@ -20,11 +20,11 @@ namespace Entities ownOrientation_.setHeading(0); ownOrientation_.setPitch(0); - ownPosition_.setGeodesicPos(0,0,0); + ownPosition_->setGeodesicPos(0,0,0); } - Movement::Movement(SimCore::Position pos):ownPosition_(pos) + Movement::Movement(std::shared_ptr pos):ownPosition_(pos) { course_ = 0; speed_= 0; @@ -37,7 +37,7 @@ namespace Entities } - Movement::Movement(SimCore::Position pos, double course):ownPosition_(pos),course_(course) + Movement::Movement(std::shared_ptr pos, double course):ownPosition_(pos),course_(course) { speed_ = 0; pitch_ = 0; @@ -46,7 +46,7 @@ namespace Entities } - Movement::Movement(SimCore::Position pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed) + Movement::Movement(std::shared_ptr pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed) { pitch_ = 0; ownOrientation_.setRoll(0); @@ -54,7 +54,7 @@ namespace Entities } - Movement::Movement(SimCore::Position pos, double course, double speed, double pitch):ownPosition_(pos),course_(course),speed_(speed),pitch_(pitch) + Movement::Movement(std::shared_ptr pos, double course, double speed, double pitch):ownPosition_(pos),course_(course),speed_(speed),pitch_(pitch) { ownOrientation_.setRoll(0); } @@ -63,15 +63,15 @@ namespace Entities void Movement::updatePosition(double dt) { - if (ownPosition_.isValid()) { + if (ownPosition_->isValid()) { calKinematicVector(); // Create ECEF coordinate system based on reference point - double lat = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LATITUDE)); // in radians - double lon = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LONGITUDE)); // in radians - double alt = ownPosition_.getGeodesicPos()(SimCore::HEIGHT); // in meters + double lat = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LATITUDE)); // in radians + double lon = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LONGITUDE)); // in radians + double alt = ownPosition_->getGeodesicPos()(SimCore::HEIGHT); // in meters Matrix3d RotationMatrix = getRotationMatrix(lat, lon); @@ -79,9 +79,9 @@ namespace Entities double X,Y,Z; - X = ownPosition_.getGeocentricPos()(SimCore::X) + ecefVelocityVector.x() * (dt / 1000.0) + 0.5 * accelerationVec_.x() * pow((dt / 1000.0), 2); - Y = ownPosition_.getGeocentricPos()(SimCore::Y) + ecefVelocityVector.y() * (dt / 1000.0) + 0.5 * accelerationVec_.y() * pow((dt / 1000.0), 2); - Z = ownPosition_.getGeocentricPos()(SimCore::Z) + ecefVelocityVector.z() * (dt / 1000.0) + 0.5 * accelerationVec_.z() * pow((dt / 1000.0), 2); + X = ownPosition_->getGeocentricPos()(SimCore::X) + ecefVelocityVector.x() * (dt / 1000.0) + 0.5 * accelerationVec_.x() * pow((dt / 1000.0), 2); + Y = ownPosition_->getGeocentricPos()(SimCore::Y) + ecefVelocityVector.y() * (dt / 1000.0) + 0.5 * accelerationVec_.y() * pow((dt / 1000.0), 2); + Z = ownPosition_->getGeocentricPos()(SimCore::Z) + ecefVelocityVector.z() * (dt / 1000.0) + 0.5 * accelerationVec_.z() * pow((dt / 1000.0), 2); std::lock_guard lock(mx); @@ -89,7 +89,7 @@ namespace Entities kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0; kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0; - ownPosition_.setGeocentricPos(X, Y, Z); + ownPosition_->setGeocentricPos(X, Y, Z); c.notify_one(); } @@ -100,28 +100,25 @@ namespace Entities void Movement::updatePositionSimple(double dt) { - double lat = ownPosition_.getGeodesicPos()(SimCore::LATITUDE); // in degree - double lon = ownPosition_.getGeodesicPos()(SimCore::LONGITUDE); // in degree - double alt = ownPosition_.getGeodesicPos()(SimCore::HEIGHT); // in meters + double lat = ownPosition_->getGeodesicPos()(SimCore::LATITUDE); // in degree + double lon = ownPosition_->getGeodesicPos()(SimCore::LONGITUDE); // in degree + double alt = ownPosition_->getGeodesicPos()(SimCore::HEIGHT); // in meters double vx = this->speed_ * cos(this->getPitch()); double vy = this->speed_ * sin(this->getPitch()); - LOG_S(INFO)<<"VX: " << vx << " VY: " <course_,movedDistance,lat2,lon2); - LOG_S(INFO)<< "lat: "<setGeodesicPos(lat2,lon2,climedHeight); } @@ -174,7 +171,7 @@ namespace Entities } - SimCore::Position Movement::getPosition() + std::shared_ptr Movement::getPosition() { std::lock_guard lock(mx); @@ -185,13 +182,13 @@ namespace Entities { std::lock_guard lock(mx); - ownPosition_.setGeodesicPos(lat, lon, height); + ownPosition_->setGeodesicPos(lat, lon, height); } void Movement::setPosition(SimCore::Position pos) { std::lock_guard lock(mx); - ownPosition_ = pos; + ownPosition_ = std::make_shared(pos); } SimCore::Orientation Movement::getownOrientation() @@ -204,7 +201,7 @@ namespace Entities Eigen::Vector3d Movement::getEulerAngles() { std::lock_guard lock(mx); - return ownOrientation_.getEulerAngles(ownPosition_.getGeodesicPos()(SimCore::LATITUDE), ownPosition_.getGeodesicPos()(SimCore::LONGITUDE)); + return ownOrientation_.getEulerAngles(ownPosition_->getGeodesicPos()(SimCore::LATITUDE), ownPosition_->getGeodesicPos()(SimCore::LONGITUDE)); }