00001 00013 #ifndef RAIL_PICK_AND_PLACE_GRASPDB_POSE_H_ 00014 #define RAIL_PICK_AND_PLACE_GRASPDB_POSE_H_ 00015 00016 // graspdb 00017 #include "Orientation.h" 00018 #include "Position.h" 00019 00020 // ROS 00021 #include <geometry_msgs/Pose.h> 00022 #include <geometry_msgs/PoseStamped.h> 00023 #include <geometry_msgs/PoseWithCovariance.h> 00024 #include <geometry_msgs/PoseWithCovarianceStamped.h> 00025 #include <geometry_msgs/Transform.h> 00026 #include <geometry_msgs/TransformStamped.h> 00027 #include <tf2/LinearMath/Transform.h> 00028 00029 // C++ Standard Library 00030 #include <string> 00031 00032 namespace rail 00033 { 00034 namespace pick_and_place 00035 { 00036 namespace graspdb 00037 { 00038 00047 class Pose 00048 { 00049 public: 00059 Pose(const std::string &robot_fixed_frame_id = "", const Position &position = Position(), 00060 const Orientation &orientation = Orientation()); 00061 00070 Pose(const std::string &robot_fixed_frame_id, const geometry_msgs::Pose &pose); 00071 00080 Pose(const geometry_msgs::PoseStamped &pose); 00081 00091 Pose(const std::string &robot_fixed_frame_id, const geometry_msgs::PoseWithCovariance &pose); 00092 00101 Pose(const geometry_msgs::PoseWithCovarianceStamped &pose); 00102 00111 Pose(const std::string &robot_fixed_frame_id, const geometry_msgs::Transform &tf); 00112 00121 Pose(const geometry_msgs::TransformStamped &tf); 00122 00131 Pose(const std::string &robot_fixed_frame_id, const tf2::Transform &tf); 00132 00140 void setRobotFixedFrameID(const std::string &robot_fixed_frame_id); 00141 00149 const std::string &getRobotFixedFrameID() const; 00150 00158 void setPosition(const Position &position); 00159 00167 const Position &getPosition() const; 00168 00176 Position &getPosition(); 00177 00185 void setOrientation(const Orientation &orientation); 00186 00194 const Orientation &getOrientation() const; 00195 00203 Orientation &getOrientation(); 00204 00210 geometry_msgs::Pose toROSPoseMessage() const; 00211 00217 geometry_msgs::PoseStamped toROSPoseStampedMessage() const; 00218 00224 geometry_msgs::Transform toROSTransformMessage() const; 00225 00231 tf2::Transform toTF2Transform() const; 00232 00233 private: 00235 std::string robot_fixed_frame_id_; 00237 Position position_; 00239 Orientation orientation_; 00240 }; 00241 00242 } 00243 } 00244 } 00245 00246 #endif