00001 00012 // worldlib 00013 #include "worldlib/geometry/Pose.h" 00014 00015 using namespace std; 00016 using namespace rail::spatial_temporal_learning::worldlib::geometry; 00017 00018 Pose::Pose(const Position &position, const Orientation &orientation) : position_(position), orientation_(orientation) 00019 { 00020 } 00021 00022 Pose::Pose(const geometry_msgs::Pose &pose) : position_(pose.position), orientation_(pose.orientation) 00023 { 00024 } 00025 00026 Pose::Pose(const geometry_msgs::Transform &tf) : position_(tf.translation), orientation_(tf.rotation) 00027 { 00028 } 00029 00030 Pose::Pose(const tf2::Transform &tf) : position_(tf.getOrigin()), orientation_(tf.getRotation()) 00031 { 00032 } 00033 00034 void Pose::setPosition(const Position &position) 00035 { 00036 position_ = position; 00037 } 00038 00039 const Position &Pose::getPosition() const 00040 { 00041 return position_; 00042 } 00043 00044 Position &Pose::getPosition() 00045 { 00046 return position_; 00047 } 00048 00049 void Pose::setOrientation(const Orientation &orientation) 00050 { 00051 orientation_ = orientation; 00052 } 00053 00054 const Orientation &Pose::getOrientation() const 00055 { 00056 return orientation_; 00057 } 00058 00059 Orientation &Pose::getOrientation() 00060 { 00061 return orientation_; 00062 } 00063 00064 geometry_msgs::Pose Pose::toROSPoseMessage() const 00065 { 00066 geometry_msgs::Pose pose; 00067 pose.position = position_.toROSPointMessage(); 00068 pose.orientation = orientation_.toROSQuaternionMessage(); 00069 return pose; 00070 } 00071 00072 tf2::Transform Pose::toTF2Transform() const 00073 { 00074 tf2::Quaternion orientation = orientation_.toTF2Quaternion(); 00075 tf2::Vector3 translation = position_.toTF2Vector3(); 00076 tf2::Transform tf(orientation, translation); 00077 return tf; 00078 }