Go to the documentation of this file.00001
00013
00014 #include "graspdb/Pose.h"
00015
00016 using namespace std;
00017 using namespace rail::pick_and_place::graspdb;
00018
00019 Pose::Pose(const string &robot_fixed_frame_id, const Position &position, const Orientation &orientation)
00020 : robot_fixed_frame_id_(robot_fixed_frame_id), position_(position), orientation_(orientation)
00021 {
00022 }
00023
00024 Pose::Pose(const string &robot_fixed_frame_id, const geometry_msgs::Pose &pose)
00025 : robot_fixed_frame_id_(robot_fixed_frame_id), position_(pose.position), orientation_(pose.orientation)
00026 {
00027 }
00028
00029 Pose::Pose(const geometry_msgs::PoseStamped &pose)
00030 : robot_fixed_frame_id_(pose.header.frame_id), position_(pose.pose.position), orientation_(pose.pose.orientation)
00031 {
00032 }
00033
00034 Pose::Pose(const string &robot_fixed_frame_id, const geometry_msgs::PoseWithCovariance &pose)
00035 : robot_fixed_frame_id_(robot_fixed_frame_id), position_(pose.pose.position), orientation_(pose.pose.orientation)
00036 {
00037 }
00038
00039 Pose::Pose(const geometry_msgs::PoseWithCovarianceStamped &pose)
00040 : robot_fixed_frame_id_(pose.header.frame_id), position_(pose.pose.pose.position),
00041 orientation_(pose.pose.pose.orientation)
00042 {
00043 }
00044
00045 Pose::Pose(const string &robot_fixed_frame_id, const geometry_msgs::Transform &tf)
00046 : robot_fixed_frame_id_(robot_fixed_frame_id), position_(tf.translation), orientation_(tf.rotation)
00047 {
00048 }
00049
00050 Pose::Pose(const geometry_msgs::TransformStamped &tf)
00051 : robot_fixed_frame_id_(tf.header.frame_id), position_(tf.transform.translation),
00052 orientation_(tf.transform.rotation)
00053 {
00054 }
00055
00056 Pose::Pose(const std::string &robot_fixed_frame_id, const tf2::Transform &tf)
00057 : robot_fixed_frame_id_(robot_fixed_frame_id), position_(tf.getOrigin()), orientation_(tf.getRotation())
00058 {
00059 }
00060
00061 void Pose::setRobotFixedFrameID(const string &robot_fixed_frame_id)
00062 {
00063 robot_fixed_frame_id_ = robot_fixed_frame_id;
00064 }
00065
00066 const string &Pose::getRobotFixedFrameID() const
00067 {
00068 return robot_fixed_frame_id_;
00069 }
00070
00071 void Pose::setPosition(const Position &position)
00072 {
00073 position_ = position;
00074 }
00075
00076 const Position &Pose::getPosition() const
00077 {
00078 return position_;
00079 }
00080
00081 Position &Pose::getPosition()
00082 {
00083 return position_;
00084 }
00085
00086 void Pose::setOrientation(const Orientation &orientation)
00087 {
00088 orientation_ = orientation;
00089 }
00090
00091 const Orientation &Pose::getOrientation() const
00092 {
00093 return orientation_;
00094 }
00095
00096 Orientation &Pose::getOrientation()
00097 {
00098 return orientation_;
00099 }
00100
00101 geometry_msgs::Pose Pose::toROSPoseMessage() const
00102 {
00103 geometry_msgs::Pose pose;
00104 pose.position = position_.toROSPointMessage();
00105 pose.orientation = orientation_.toROSQuaternionMessage();
00106 return pose;
00107 }
00108
00109 geometry_msgs::PoseStamped Pose::toROSPoseStampedMessage() const
00110 {
00111 geometry_msgs::PoseStamped pose;
00112 pose.header.frame_id = robot_fixed_frame_id_;
00113 pose.pose.position = position_.toROSPointMessage();
00114 pose.pose.orientation = orientation_.toROSQuaternionMessage();
00115 return pose;
00116 }
00117
00118 tf2::Transform Pose::toTF2Transform() const
00119 {
00120 tf2::Quaternion orientation = orientation_.toTF2Quaternion();
00121 tf2::Vector3 translation = position_.toTF2Vector3();
00122 tf2::Transform tf(orientation, translation);
00123 return tf;
00124 }