Go to the documentation of this file.00001
00012 #ifndef RAIL_PICK_AND_PLACE_GRASPDB_ORIENTATION_H_
00013 #define RAIL_PICK_AND_PLACE_GRASPDB_ORIENTATION_H_
00014
00015
00016 #include <geometry_msgs/Quaternion.h>
00017 #include <tf2/LinearMath/Matrix3x3.h>
00018 #include <tf2/LinearMath/Quaternion.h>
00019
00020
00021 #include <string>
00022
00023 namespace rail
00024 {
00025 namespace pick_and_place
00026 {
00027 namespace graspdb
00028 {
00029
00037 class Orientation
00038 {
00039 public:
00050 Orientation(const double x = 0, const double y = 0, const double z = 0, const double w = 0);
00051
00059 Orientation(const geometry_msgs::Quaternion &quaternion);
00060
00068 Orientation(const tf2::Quaternion &quaternion);
00069
00077 void setX(const double x);
00078
00086 double getX() const;
00087
00095 void setY(const double y);
00096
00104 double getY() const;
00105
00113 void setZ(const double z);
00114
00122 double getZ() const;
00123
00131 void setW(const double w);
00132
00140 double getW() const;
00141
00147 geometry_msgs::Quaternion toROSQuaternionMessage() const;
00148
00154 tf2::Quaternion toTF2Quaternion() const;
00155
00161 tf2::Matrix3x3 toTF2Matrix3x3() const;
00162
00163 private:
00165 double x_, y_, z_, w_;
00166 };
00167
00168 }
00169 }
00170 }
00171
00172 #endif