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 robot_state
00048 {
00049
00050 MOVEIT_CLASS_FORWARD(Transforms);
00051
00053 typedef std::map<std::string, Eigen::Affine3d, std::less<std::string>,
00054 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3d> > > FixedTransformsMap;
00055
00059 class Transforms : private boost::noncopyable
00060 {
00061 public:
00065 Transforms(const std::string &target_frame);
00066
00070 virtual ~Transforms();
00071
00073 static bool sameFrame(const std::string &frame1, const std::string &frame2);
00074
00079 const std::string& getTargetFrame() const;
00080
00090 const FixedTransformsMap& getAllTransforms() const;
00091
00096 void copyTransforms(std::vector<geometry_msgs::TransformStamped> &transforms) const;
00097
00103 void setTransform(const Eigen::Affine3d &t, const std::string &from_frame);
00104
00109 void setTransform(const geometry_msgs::TransformStamped &transform);
00110
00115 void setTransforms(const std::vector<geometry_msgs::TransformStamped> &transforms);
00116
00120 void setAllTransforms(const FixedTransformsMap &transforms);
00121
00135 void transformVector3(const std::string &from_frame, const Eigen::Vector3d &v_in, Eigen::Vector3d &v_out) const
00136 {
00137 v_out = getTransform(from_frame).rotation() * v_in;
00138 }
00139
00146 void transformQuaternion(const std::string &from_frame, const Eigen::Quaterniond &q_in, Eigen::Quaterniond &q_out) const
00147 {
00148 q_out = getTransform(from_frame).rotation() * q_in;
00149 }
00150
00157 void transformRotationMatrix(const std::string &from_frame, const Eigen::Matrix3d &m_in, Eigen::Matrix3d &m_out) const
00158 {
00159 m_out = getTransform(from_frame).rotation() * m_in;
00160 }
00161
00168 void transformPose(const std::string &from_frame, const Eigen::Affine3d &t_in, Eigen::Affine3d &t_out) const
00169 {
00170 t_out = getTransform(from_frame) * t_in;
00171 }
00177 virtual bool canTransform(const std::string &from_frame) const;
00178
00182 virtual bool isFixedFrame(const std::string &frame) const;
00183
00189 virtual const Eigen::Affine3d& getTransform(const std::string &from_frame) const;
00190
00191 protected:
00192
00193 std::string target_frame_;
00194 FixedTransformsMap transforms_;
00195
00196 public:
00197 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00198 };
00199
00200 }
00201
00202 #endif