Go to the documentation of this file.00001
00012
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
00021 x_ = x;
00022 y_ = y;
00023 z_ = z;
00024 }
00025
00026 Position::Position(const geometry_msgs::Point &point)
00027 {
00028
00029 x_ = point.x;
00030 y_ = point.y;
00031 z_ = point.z;
00032 }
00033
00034 Position::Position(const geometry_msgs::Vector3 &v)
00035 {
00036
00037 x_ = v.x;
00038 y_ = v.y;
00039 z_ = v.z;
00040 }
00041
00042 Position::Position(const tf2::Vector3 &v)
00043 {
00044
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
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