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/noncopyable.hpp>
00044 #include <moveit/macros/class_forward.h>
00045
00046 namespace moveit
00047 {
00048 namespace core
00049 {
00050 MOVEIT_CLASS_FORWARD(Transforms);
00051
00054 typedef std::map<std::string, Eigen::Affine3d, std::less<std::string>,
00055 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3d> > >
00056 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
00123 void setAllTransforms(const FixedTransformsMap& transforms);
00124
00139 void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const
00140 {
00141 v_out = getTransform(from_frame).rotation() * v_in;
00142 }
00143
00150 void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in,
00151 Eigen::Quaterniond& q_out) const
00152 {
00153 q_out = getTransform(from_frame).rotation() * q_in;
00154 }
00155
00162 void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const
00163 {
00164 m_out = getTransform(from_frame).rotation() * m_in;
00165 }
00166
00173 void transformPose(const std::string& from_frame, const Eigen::Affine3d& t_in, Eigen::Affine3d& t_out) const
00174 {
00175 t_out = getTransform(from_frame) * t_in;
00176 }
00182 virtual bool canTransform(const std::string& from_frame) const;
00183
00188 virtual bool isFixedFrame(const std::string& frame) const;
00189
00195 virtual const Eigen::Affine3d& getTransform(const std::string& from_frame) const;
00196
00197 protected:
00198 std::string target_frame_;
00199 FixedTransformsMap transforms_;
00200 };
00201 }
00202 }
00203
00204 #endif