transforms.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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 moveit::core::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 moveit::core::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 moveit::core::Transforms::~Transforms()
00070 {
00071 }
00072 
00073 const std::string& moveit::core::Transforms::getTargetFrame() const
00074 {
00075   return target_frame_;
00076 }
00077 
00078 const moveit::core::FixedTransformsMap& moveit::core::Transforms::getAllTransforms() const
00079 {
00080   return transforms_;
00081 }
00082 
00083 void moveit::core::Transforms::setAllTransforms(const FixedTransformsMap &transforms)
00084 {
00085   transforms_ = transforms;
00086 }
00087 
00088 bool moveit::core::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& moveit::core::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   // return identity
00108   static const Eigen::Affine3d identity = Eigen::Affine3d::Identity();
00109   return identity;
00110 }
00111 
00112 bool moveit::core::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 moveit::core::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 moveit::core::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 moveit::core::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 moveit::core::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 }


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Thu Aug 27 2015 13:58:53