00001 00012 #ifndef SPATIAL_TEMPORAL_LEARNING_WORLDLIB_GEOMETRY_ORIENTATION_H_ 00013 #define SPATIAL_TEMPORAL_LEARNING_WORLDLIB_GEOMETRY_ORIENTATION_H_ 00014 00015 // ROS 00016 #include <geometry_msgs/Quaternion.h> 00017 #include <tf2/LinearMath/Matrix3x3.h> 00018 #include <tf2/LinearMath/Quaternion.h> 00019 00020 namespace rail 00021 { 00022 namespace spatial_temporal_learning 00023 { 00024 namespace worldlib 00025 { 00026 namespace geometry 00027 { 00028 00036 class Orientation 00037 { 00038 public: 00046 Orientation(const double theta = 0); 00047 00058 Orientation(const double x, const double y, const double z, const double w); 00059 00067 Orientation(const geometry_msgs::Quaternion &quaternion); 00068 00076 Orientation(const tf2::Quaternion &quaternion); 00077 00085 void setX(const double x); 00086 00094 double getX() const; 00095 00103 void setY(const double y); 00104 00112 double getY() const; 00113 00121 void setZ(const double z); 00122 00130 double getZ() const; 00131 00139 void setW(const double w); 00140 00148 double getW() const; 00149 00157 double getTheta() const; 00158 00164 geometry_msgs::Quaternion toROSQuaternionMessage() const; 00165 00171 tf2::Quaternion toTF2Quaternion() const; 00172 00178 tf2::Matrix3x3 toTF2Matrix3x3() const; 00179 00180 private: 00182 double x_, y_, z_, w_; 00183 }; 00184 00185 } 00186 } 00187 } 00188 } 00189 00190 #endif