Position.cpp
Go to the documentation of this file.
00001 
00012 // worldlib
00013 #include "worldlib/geometry/Position.h"
00014 
00015 using namespace std;
00016 using namespace rail::spatial_temporal_learning::worldlib::geometry;
00017 
00018 Position::Position(const double x, const double y, const double z)
00019 {
00020   // set position data
00021   x_ = x;
00022   y_ = y;
00023   z_ = z;
00024 }
00025 
00026 Position::Position(const geometry_msgs::Point &point)
00027 {
00028   // copy position data
00029   x_ = point.x;
00030   y_ = point.y;
00031   z_ = point.z;
00032 }
00033 
00034 Position::Position(const geometry_msgs::Vector3 &v)
00035 {
00036   // copy position data
00037   x_ = v.x;
00038   y_ = v.y;
00039   z_ = v.z;
00040 }
00041 
00042 Position::Position(const tf2::Vector3 &v)
00043 {
00044   // copy position data
00045   x_ = v.getX();
00046   y_ = v.getY();
00047   z_ = v.getZ();
00048 }
00049 
00050 void Position::setX(const double x)
00051 {
00052   x_ = x;
00053 }
00054 
00055 double Position::getX() const
00056 {
00057   return x_;
00058 }
00059 
00060 void Position::setY(const double y)
00061 {
00062   y_ = y;
00063 }
00064 
00065 double Position::getY() const
00066 {
00067   return y_;
00068 }
00069 
00070 void Position::setZ(const double z)
00071 {
00072   z_ = z;
00073 }
00074 
00075 double Position::getZ() const
00076 {
00077   return z_;
00078 }
00079 
00080 double Position::distance(const Position &position) const
00081 {
00082   // use TF2 methods
00083   return this->toTF2Vector3().distance(position.toTF2Vector3());
00084 }
00085 
00086 geometry_msgs::Point Position::toROSPointMessage() const
00087 {
00088   geometry_msgs::Point p;
00089   p.x = x_;
00090   p.y = y_;
00091   p.z = z_;
00092   return p;
00093 }
00094 
00095 geometry_msgs::Vector3 Position::toROSVector3Message() const
00096 {
00097   geometry_msgs::Vector3 v;
00098   v.x = x_;
00099   v.y = y_;
00100   v.z = z_;
00101   return v;
00102 }
00103 
00104 tf2::Vector3 Position::toTF2Vector3() const
00105 {
00106   tf2::Vector3 v(x_, y_, z_);
00107   return v;
00108 }
00109 


worldlib
Author(s): Russell Toris
autogenerated on Thu Jun 6 2019 20:55:36