Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_TRANSFORMS_
00038 #define MOVEIT_TRANSFORMS_
00039
00040 #include <geometry_msgs/TransformStamped.h>
00041 #include <geometry_msgs/Pose.h>
00042 #include <Eigen/Geometry>
00043 #include <boost/shared_ptr.hpp>
00044 #include <boost/noncopyable.hpp>
00045 #include <moveit/macros/class_forward.h>
00046
00047 namespace moveit
00048 {
00049 namespace core
00050 {
00051
00052 MOVEIT_CLASS_FORWARD(Transforms);
00053
00055 typedef std::map<std::string, Eigen::Affine3d, std::less<std::string>,
00056 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3d> > > FixedTransformsMap;
00057
00061 class Transforms : private boost::noncopyable
00062 {
00063 public:
00067 Transforms(const std::string &target_frame);
00068
00072 virtual ~Transforms();
00073
00075 static bool sameFrame(const std::string &frame1, const std::string &frame2);
00076
00081 const std::string& getTargetFrame() const;
00082
00092 const FixedTransformsMap& getAllTransforms() const;
00093
00098 void copyTransforms(std::vector<geometry_msgs::TransformStamped> &transforms) const;
00099
00105 void setTransform(const Eigen::Affine3d &t, const std::string &from_frame);
00106
00111 void setTransform(const geometry_msgs::TransformStamped &transform);
00112
00117 void setTransforms(const std::vector<geometry_msgs::TransformStamped> &transforms);
00118
00122 void setAllTransforms(const FixedTransformsMap &transforms);
00123
00137 void transformVector3(const std::string &from_frame, const Eigen::Vector3d &v_in, Eigen::Vector3d &v_out) const
00138 {
00139 v_out = getTransform(from_frame).rotation() * v_in;
00140 }
00141
00148 void transformQuaternion(const std::string &from_frame, const Eigen::Quaterniond &q_in, Eigen::Quaterniond &q_out) const
00149 {
00150 q_out = getTransform(from_frame).rotation() * q_in;
00151 }
00152
00159 void transformRotationMatrix(const std::string &from_frame, const Eigen::Matrix3d &m_in, Eigen::Matrix3d &m_out) const
00160 {
00161 m_out = getTransform(from_frame).rotation() * m_in;
00162 }
00163
00170 void transformPose(const std::string &from_frame, const Eigen::Affine3d &t_in, Eigen::Affine3d &t_out) const
00171 {
00172 t_out = getTransform(from_frame) * t_in;
00173 }
00179 virtual bool canTransform(const std::string &from_frame) const;
00180
00185 virtual bool isFixedFrame(const std::string &frame) const;
00186
00192 virtual const Eigen::Affine3d& getTransform(const std::string &from_frame) const;
00193
00194 protected:
00195
00196 std::string target_frame_;
00197 FixedTransformsMap transforms_;
00198
00199 };
00200
00201 }
00202 }
00203
00204 #endif