00001 00012 #ifndef SPATIAL_TEMPORAL_LEARNING_WORLDLIB_GEOMETRY_POSE_H_ 00013 #define SPATIAL_TEMPORAL_LEARNING_WORLDLIB_GEOMETRY_POSE_H_ 00014 00015 // worldlib 00016 #include "Orientation.h" 00017 #include "Position.h" 00018 00019 // ROS 00020 #include <geometry_msgs/Pose.h> 00021 #include <geometry_msgs/Transform.h> 00022 #include <tf2/LinearMath/Transform.h> 00023 00024 namespace rail 00025 { 00026 namespace spatial_temporal_learning 00027 { 00028 namespace worldlib 00029 { 00030 namespace geometry 00031 { 00032 00040 class Pose 00041 { 00042 public: 00051 Pose(const Position &position = Position(), const Orientation &orientation = Orientation()); 00052 00060 Pose(const geometry_msgs::Pose &pose); 00061 00069 Pose(const geometry_msgs::Transform &tf); 00070 00078 Pose(const tf2::Transform &tf); 00079 00087 void setPosition(const Position &position); 00088 00096 const Position &getPosition() const; 00097 00105 Position &getPosition(); 00106 00114 void setOrientation(const Orientation &orientation); 00115 00123 const Orientation &getOrientation() const; 00124 00132 Orientation &getOrientation(); 00133 00139 geometry_msgs::Pose toROSPoseMessage() const; 00140 00146 geometry_msgs::Transform toROSTransformMessage() const; 00147 00153 tf2::Transform toTF2Transform() const; 00154 00155 private: 00157 Position position_; 00159 Orientation orientation_; 00160 }; 00161 00162 } 00163 } 00164 } 00165 } 00166 00167 #endif