Pose.cpp
Go to the documentation of this file.
00001 
00013 // graspdb
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 }


graspdb
Author(s): Russell Toris
autogenerated on Sun Mar 6 2016 11:38:59