Orientation.cpp
Go to the documentation of this file.
00001 
00012 // worldlib
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   // use TF2 to help us out
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   // set orientation data
00030   x_ = x;
00031   y_ = y;
00032   z_ = z;
00033   w_ = w;
00034 }
00035 
00036 Orientation::Orientation(const geometry_msgs::Quaternion &quaternion)
00037 {
00038   // copy position data
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   // copy position data
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   // use the TF2 method
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 }


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