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

CHG: movement now uses sharedptr for the position

Browse Source
This commit is contained in:
hwinkel
2023-05-27 21:34:53 +02:00
parent 973e8a263f
commit 10f92fb966
3 changed files with 31 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
include/Entities/Movement.hpp
View File

@@ -25,10 +25,10 @@ namespace Entities
{ {
public: public:
Movement(); Movement();
Movement(SimCore::Position pos); Movement(std::shared_ptr<SimCore::Position> pos);
Movement(SimCore::Position pos, double course); Movement(std::shared_ptr<SimCore::Position>, double course);
Movement(SimCore::Position pos, double course, double speed); Movement(std::shared_ptr<SimCore::Position>, double course, double speed);
Movement(SimCore::Position pos, double course, double speed, double pitch); Movement(std::shared_ptr<SimCore::Position>, double course, double speed, double pitch);
@@ -69,7 +69,7 @@ namespace Entities
*/ */
Eigen::Vector3d getKinematicVector(); Eigen::Vector3d getKinematicVector();
SimCore::Position getPosition(); std::shared_ptr<SimCore::Position> getPosition();
void setPosition(double lat, double lon, double height = 0); void setPosition(double lat, double lon, double height = 0);
@@ -85,7 +85,7 @@ namespace Entities
private: private:
SimCore::Position ownPosition_; std::shared_ptr<SimCore::Position> ownPosition_;
SimCore::Orientation ownOrientation_; SimCore::Orientation ownOrientation_;
Eigen::Vector3d kinematicVec_; Eigen::Vector3d kinematicVec_;

6
src/Entities/Entity.cpp
View File

@@ -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)); std::this_thread::sleep_for(std::chrono::milliseconds(900));
} }
@@ -169,7 +169,7 @@ namespace Entities
while (!stopSensorWorker) 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)); std::this_thread::sleep_for(std::chrono::milliseconds(900));
} }
@@ -180,7 +180,7 @@ namespace Entities
{ {
while (!stopTrackWorker) 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)); std::this_thread::sleep_for(std::chrono::milliseconds(900));
} }

47
src/Entities/Movement.cpp
View File

@@ -20,11 +20,11 @@ namespace Entities
ownOrientation_.setHeading(0); ownOrientation_.setHeading(0);
ownOrientation_.setPitch(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<SimCore::Position> pos):ownPosition_(pos)
{ {
course_ = 0; course_ = 0;
speed_= 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<SimCore::Position> pos, double course):ownPosition_(pos),course_(course)
{ {
speed_ = 0; speed_ = 0;
pitch_ = 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<SimCore::Position> pos, double course, double speed):ownPosition_(pos),course_(course),speed_(speed)
{ {
pitch_ = 0; pitch_ = 0;
ownOrientation_.setRoll(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<SimCore::Position> pos, double course, double speed, double pitch):ownPosition_(pos),course_(course),speed_(speed),pitch_(pitch)
{ {
ownOrientation_.setRoll(0); ownOrientation_.setRoll(0);
} }
@@ -63,15 +63,15 @@ namespace Entities
void Movement::updatePosition(double dt) void Movement::updatePosition(double dt)
{ {
if (ownPosition_.isValid()) { if (ownPosition_->isValid()) {
calKinematicVector(); calKinematicVector();
// Create ECEF coordinate system based on reference point // Create ECEF coordinate system based on reference point
double lat = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LATITUDE)); // in radians double lat = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LATITUDE)); // in radians
double lon = SimCore::UtilFunctions::DegToRad(ownPosition_.getGeodesicPos()(SimCore::LONGITUDE)); // in radians double lon = SimCore::UtilFunctions::DegToRad(ownPosition_->getGeodesicPos()(SimCore::LONGITUDE)); // in radians
double alt = ownPosition_.getGeodesicPos()(SimCore::HEIGHT); // in meters double alt = ownPosition_->getGeodesicPos()(SimCore::HEIGHT); // in meters
Matrix3d RotationMatrix = getRotationMatrix(lat, lon); Matrix3d RotationMatrix = getRotationMatrix(lat, lon);
@@ -79,9 +79,9 @@ namespace Entities
double X,Y,Z; double X,Y,Z;
X = ownPosition_.getGeocentricPos()(SimCore::X) + ecefVelocityVector.x() * (dt / 1000.0) + 0.5 * accelerationVec_.x() * 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); 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); Z = ownPosition_->getGeocentricPos()(SimCore::Z) + ecefVelocityVector.z() * (dt / 1000.0) + 0.5 * accelerationVec_.z() * pow((dt / 1000.0), 2);
std::lock_guard<std::mutex> lock(mx); std::lock_guard<std::mutex> lock(mx);
@@ -89,7 +89,7 @@ namespace Entities
kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0; kinematicVec_.y() += accelerationVec_.y() * dt / 1000.0;
kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0; kinematicVec_.z() += accelerationVec_.z() * dt / 1000.0;
ownPosition_.setGeocentricPos(X, Y, Z); ownPosition_->setGeocentricPos(X, Y, Z);
c.notify_one(); c.notify_one();
} }
@@ -100,28 +100,25 @@ namespace Entities
void Movement::updatePositionSimple(double dt) void Movement::updatePositionSimple(double dt)
{ {
double lat = ownPosition_.getGeodesicPos()(SimCore::LATITUDE); // in degree double lat = ownPosition_->getGeodesicPos()(SimCore::LATITUDE); // in degree
double lon = ownPosition_.getGeodesicPos()(SimCore::LONGITUDE); // in degree double lon = ownPosition_->getGeodesicPos()(SimCore::LONGITUDE); // in degree
double alt = ownPosition_.getGeodesicPos()(SimCore::HEIGHT); // in meters double alt = ownPosition_->getGeodesicPos()(SimCore::HEIGHT); // in meters
double vx = this->speed_ * cos(this->getPitch()); double vx = this->speed_ * cos(this->getPitch());
double vy = this->speed_ * sin(this->getPitch()); double vy = this->speed_ * sin(this->getPitch());
LOG_S(INFO)<<"VX: " << vx << " VY: " <<vy;
double movedDistance = vx * dt / 1000; double movedDistance = vx * dt / 1000;
double climedHeight = vy * dt / 1000; double climedHeight = vy * dt / 1000;
LOG_S(INFO)<<"moved distance: " <<movedDistance;
GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
double lat2,lon2; double lat2,lon2;
geod.Direct(lat,lon,this->course_,movedDistance,lat2,lon2); geod.Direct(lat,lon,this->course_,movedDistance,lat2,lon2);
LOG_S(INFO)<< "lat: "<<lat<<" lon: "<<lon;
LOG_S(INFO)<< "lat: "<<lat2<<" lon: "<<lon2; ownPosition_->setGeodesicPos(lat2,lon2,climedHeight);
ownPosition_.setGeodesicPos(lat2,lon2,climedHeight);
} }
@@ -174,7 +171,7 @@ namespace Entities
} }
SimCore::Position Movement::getPosition() std::shared_ptr<SimCore::Position> Movement::getPosition()
{ {
std::lock_guard<std::mutex> lock(mx); std::lock_guard<std::mutex> lock(mx);
@@ -185,13 +182,13 @@ namespace Entities
{ {
std::lock_guard<std::mutex> lock(mx); std::lock_guard<std::mutex> lock(mx);
ownPosition_.setGeodesicPos(lat, lon, height); ownPosition_->setGeodesicPos(lat, lon, height);
} }
void Movement::setPosition(SimCore::Position pos) void Movement::setPosition(SimCore::Position pos)
{ {
std::lock_guard<std::mutex> lock(mx); std::lock_guard<std::mutex> lock(mx);
ownPosition_ = pos; ownPosition_ = std::make_shared<SimCore::Position>(pos);
} }
SimCore::Orientation Movement::getownOrientation() SimCore::Orientation Movement::getownOrientation()
@@ -204,7 +201,7 @@ namespace Entities
Eigen::Vector3d Movement::getEulerAngles() Eigen::Vector3d Movement::getEulerAngles()
{ {
std::lock_guard<std::mutex> lock(mx); std::lock_guard<std::mutex> lock(mx);
return ownOrientation_.getEulerAngles(ownPosition_.getGeodesicPos()(SimCore::LATITUDE), ownPosition_.getGeodesicPos()(SimCore::LONGITUDE)); return ownOrientation_.getEulerAngles(ownPosition_->getGeodesicPos()(SimCore::LATITUDE), ownPosition_->getGeodesicPos()(SimCore::LONGITUDE));
} }