Go to the documentation of this file.00001
00012
00013 #include "worldlib/geometry/Orientation.h"
00014
00015 using namespace std;
00016 using namespace rail::spatial_temporal_learning::worldlib::geometry;
00017
00018 Orientation::Orientation(const double theta) {
00019
00020 tf2::Quaternion quaternion(tf2::Vector3(1, 0, 0), theta);
00021 x_ = quaternion.getX();
00022 y_ = quaternion.getY();
00023 z_ = quaternion.getZ();
00024 w_ = quaternion.getW();
00025 }
00026
00027 Orientation::Orientation(const double x, const double y, const double z, const double w)
00028 {
00029
00030 x_ = x;
00031 y_ = y;
00032 z_ = z;
00033 w_ = w;
00034 }
00035
00036 Orientation::Orientation(const geometry_msgs::Quaternion &quaternion)
00037 {
00038
00039 x_ = quaternion.x;
00040 y_ = quaternion.y;
00041 z_ = quaternion.z;
00042 w_ = quaternion.w;
00043 }
00044
00045 Orientation::Orientation(const tf2::Quaternion &quaternion)
00046 {
00047
00048 x_ = quaternion.getX();
00049 y_ = quaternion.getY();
00050 z_ = quaternion.getZ();
00051 w_ = quaternion.getW();
00052 }
00053
00054 void Orientation::setX(const double x)
00055 {
00056 x_ = x;
00057 }
00058
00059 double Orientation::getX() const
00060 {
00061 return x_;
00062 }
00063
00064 void Orientation::setY(const double y)
00065 {
00066 y_ = y;
00067 }
00068
00069 double Orientation::getY() const
00070 {
00071 return y_;
00072 }
00073
00074 void Orientation::setZ(const double z)
00075 {
00076 z_ = z;
00077 }
00078
00079 double Orientation::getZ() const
00080 {
00081 return z_;
00082 }
00083
00084 void Orientation::setW(const double w)
00085 {
00086 w_ = w;
00087 }
00088
00089 double Orientation::getW() const
00090 {
00091 return w_;
00092 }
00093
00094 double Orientation::getTheta() const
00095 {
00096
00097 return this->toTF2Quaternion().getAngle();
00098 }
00099
00100 geometry_msgs::Quaternion Orientation::toROSQuaternionMessage() const
00101 {
00102 geometry_msgs::Quaternion q;
00103 q.x = x_;
00104 q.y = y_;
00105 q.z = z_;
00106 q.w = w_;
00107 return q;
00108 }
00109
00110 tf2::Quaternion Orientation::toTF2Quaternion() const
00111 {
00112 tf2::Quaternion q(x_, y_, z_, w_);
00113 return q;
00114 }
00115
00116 tf2::Matrix3x3 Orientation::toTF2Matrix3x3() const
00117 {
00118 tf2::Quaternion q = this->toTF2Quaternion();
00119 tf2::Matrix3x3 m(q);
00120 return m;
00121 }