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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Fri Apr 20 2018 03:31:11