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

47
src/Entities/Movement.cpp
View File

@@ -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<SimCore::Position> 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<SimCore::Position> 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<SimCore::Position> 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<SimCore::Position> 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<std::mutex> 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: " <<vy;
double movedDistance = vx * dt / 1000;
double climedHeight = vy * dt / 1000;
LOG_S(INFO)<<"moved distance: " <<movedDistance;
GeographicLib::Geodesic geod(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
double 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);
@@ -185,13 +182,13 @@ namespace Entities
{
std::lock_guard<std::mutex> lock(mx);
ownPosition_.setGeodesicPos(lat, lon, height);
ownPosition_->setGeodesicPos(lat, lon, height);
}
void Movement::setPosition(SimCore::Position pos)
{
std::lock_guard<std::mutex> lock(mx);
ownPosition_ = pos;
ownPosition_ = std::make_shared<SimCore::Position>(pos);
}
SimCore::Orientation Movement::getownOrientation()
@@ -204,7 +201,7 @@ namespace Entities
Eigen::Vector3d Movement::getEulerAngles()
{
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));
}