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 #include <moveit/transforms/transforms.h>
00038 #include <eigen_conversions/eigen_msg.h>
00039 #include <boost/algorithm/string/trim.hpp>
00040 #include <console_bridge/console.h>
00041
00042 robot_state::Transforms::Transforms(const std::string &target_frame) : target_frame_(target_frame)
00043 {
00044 boost::trim(target_frame_);
00045 if (target_frame_.empty())
00046 logError("The target frame for MoveIt Transforms cannot be empty.");
00047 else
00048 {
00049 if (target_frame_[0] != '/')
00050 {
00051 logWarn("Frame '%s' specified as target frame for MoveIt Transforms. Assuming '/%s' instead.", target_frame_.c_str(), target_frame_.c_str());
00052 target_frame_ = '/' + target_frame_;
00053 }
00054 transforms_[target_frame_] = Eigen::Affine3d::Identity();
00055 }
00056 }
00057
00058 bool robot_state::Transforms::sameFrame(const std::string &frame1, const std::string &frame2)
00059 {
00060 if (frame1.empty() || frame2.empty())
00061 return false;
00062 if (frame1[0] != '/')
00063 return sameFrame('/' + frame1, frame2);
00064 if (frame2[0] != '/')
00065 return sameFrame(frame1, '/' + frame2);
00066 return frame1 == frame2;
00067 }
00068
00069 robot_state::Transforms::~Transforms()
00070 {
00071 }
00072
00073 const std::string& robot_state::Transforms::getTargetFrame() const
00074 {
00075 return target_frame_;
00076 }
00077
00078 const robot_state::FixedTransformsMap& robot_state::Transforms::getAllTransforms() const
00079 {
00080 return transforms_;
00081 }
00082
00083 void robot_state::Transforms::setAllTransforms(const FixedTransformsMap &transforms)
00084 {
00085 transforms_ = transforms;
00086 }
00087
00088 bool robot_state::Transforms::isFixedFrame(const std::string &frame) const
00089 {
00090 if (frame.empty())
00091 return false;
00092 else
00093 return (frame[0] == '/' ? transforms_.find(frame) : transforms_.find('/' + frame)) != transforms_.end();
00094 }
00095
00096 const Eigen::Affine3d& robot_state::Transforms::getTransform(const std::string &from_frame) const
00097 {
00098 if (!from_frame.empty())
00099 {
00100 FixedTransformsMap::const_iterator it = (from_frame[0] == '/' ? transforms_.find(from_frame) : transforms_.find('/' + from_frame));
00101 if (it != transforms_.end())
00102 return it->second;
00103 }
00104
00105 logError("Unable to transform from frame '%s' to frame '%s'. Returning identity.", from_frame.c_str(), target_frame_.c_str());
00106
00107
00108 static const Eigen::Affine3d identity = Eigen::Affine3d::Identity();
00109 return identity;
00110 }
00111
00112 bool robot_state::Transforms::canTransform(const std::string &from_frame) const
00113 {
00114 if (from_frame.empty())
00115 return false;
00116 else
00117 return (from_frame[0] == '/' ? transforms_.find(from_frame) : transforms_.find('/' + from_frame)) != transforms_.end();
00118 }
00119
00120 void robot_state::Transforms::setTransform(const Eigen::Affine3d &t, const std::string &from_frame)
00121 {
00122 if (from_frame.empty())
00123 logError("Cannot record transform with empty name");
00124 else
00125 {
00126 if (from_frame[0] != '/')
00127 {
00128 logWarn("Transform specified for frame '%s'. Assuming '/%s' instead", from_frame.c_str(), from_frame.c_str());
00129 transforms_['/' + from_frame] = t;
00130 }
00131 else
00132 transforms_[from_frame] = t;
00133 }
00134 }
00135
00136 void robot_state::Transforms::setTransform(const geometry_msgs::TransformStamped &transform)
00137 {
00138 if (sameFrame(transform.child_frame_id, target_frame_))
00139 {
00140 Eigen::Affine3d t;
00141 tf::transformMsgToEigen(transform.transform, t);
00142 setTransform(t, transform.header.frame_id);
00143 }
00144 else
00145 {
00146 logError("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(), target_frame_.c_str());
00147 }
00148 }
00149
00150 void robot_state::Transforms::setTransforms(const std::vector<geometry_msgs::TransformStamped> &transforms)
00151 {
00152 for (std::size_t i = 0 ; i < transforms.size() ; ++i)
00153 setTransform(transforms[i]);
00154 }
00155
00156 void robot_state::Transforms::copyTransforms(std::vector<geometry_msgs::TransformStamped> &transforms) const
00157 {
00158 transforms.resize(transforms_.size());
00159 std::size_t i = 0;
00160 for (FixedTransformsMap::const_iterator it = transforms_.begin() ; it != transforms_.end() ; ++it, ++i)
00161 {
00162 transforms[i].child_frame_id = target_frame_;
00163 transforms[i].header.frame_id = it->first;
00164 tf::transformEigenToMsg(it->second, transforms[i].transform);
00165 }
00166 }