transforms.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
39 #include <boost/algorithm/string/trim.hpp>
40 #include <console_bridge/console.h>
41 
42 moveit::core::Transforms::Transforms(const std::string& target_frame) : target_frame_(target_frame)
43 {
44  boost::trim(target_frame_);
45  if (target_frame_.empty())
46  logError("The target frame for MoveIt Transforms cannot be empty.");
47  else
48  {
49  if (target_frame_[0] != '/')
50  {
51  logWarn("Frame '%s' specified as target frame for MoveIt Transforms. Assuming '/%s' instead.",
52  target_frame_.c_str(), target_frame_.c_str());
54  }
55  transforms_[target_frame_] = Eigen::Affine3d::Identity();
56  }
57 }
58 
59 bool moveit::core::Transforms::sameFrame(const std::string& frame1, const std::string& frame2)
60 {
61  if (frame1.empty() || frame2.empty())
62  return false;
63  if (frame1[0] != '/')
64  return sameFrame('/' + frame1, frame2);
65  if (frame2[0] != '/')
66  return sameFrame(frame1, '/' + frame2);
67  return frame1 == frame2;
68 }
69 
71 {
72 }
73 
74 const std::string& moveit::core::Transforms::getTargetFrame() const
75 {
76  return target_frame_;
77 }
78 
80 {
81  return transforms_;
82 }
83 
85 {
86  transforms_ = transforms;
87 }
88 
89 bool moveit::core::Transforms::isFixedFrame(const std::string& frame) const
90 {
91  if (frame.empty())
92  return false;
93  else
94  return (frame[0] == '/' ? transforms_.find(frame) : transforms_.find('/' + frame)) != transforms_.end();
95 }
96 
97 const Eigen::Affine3d& moveit::core::Transforms::getTransform(const std::string& from_frame) const
98 {
99  if (!from_frame.empty())
100  {
101  FixedTransformsMap::const_iterator it =
102  (from_frame[0] == '/' ? transforms_.find(from_frame) : transforms_.find('/' + from_frame));
103  if (it != transforms_.end())
104  return it->second;
105  }
106 
107  logError("Unable to transform from frame '%s' to frame '%s'. Returning identity.", from_frame.c_str(),
108  target_frame_.c_str());
109 
110  // return identity
111  static const Eigen::Affine3d identity = Eigen::Affine3d::Identity();
112  return identity;
113 }
114 
115 bool moveit::core::Transforms::canTransform(const std::string& from_frame) const
116 {
117  if (from_frame.empty())
118  return false;
119  else
120  return (from_frame[0] == '/' ? transforms_.find(from_frame) : transforms_.find('/' + from_frame)) !=
121  transforms_.end();
122 }
123 
124 void moveit::core::Transforms::setTransform(const Eigen::Affine3d& t, const std::string& from_frame)
125 {
126  if (from_frame.empty())
127  logError("Cannot record transform with empty name");
128  else
129  {
130  if (from_frame[0] != '/')
131  {
132  logWarn("Transform specified for frame '%s'. Assuming '/%s' instead", from_frame.c_str(), from_frame.c_str());
133  transforms_['/' + from_frame] = t;
134  }
135  else
136  transforms_[from_frame] = t;
137  }
138 }
139 
140 void moveit::core::Transforms::setTransform(const geometry_msgs::TransformStamped& transform)
141 {
142  if (sameFrame(transform.child_frame_id, target_frame_))
143  {
144  Eigen::Affine3d t;
145  tf::transformMsgToEigen(transform.transform, t);
146  setTransform(t, transform.header.frame_id);
147  }
148  else
149  {
150  logError("Given transform is to frame '%s', but frame '%s' was expected.", transform.child_frame_id.c_str(),
151  target_frame_.c_str());
152  }
153 }
154 
155 void moveit::core::Transforms::setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms)
156 {
157  for (std::size_t i = 0; i < transforms.size(); ++i)
158  setTransform(transforms[i]);
159 }
160 
161 void moveit::core::Transforms::copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const
162 {
163  transforms.resize(transforms_.size());
164  std::size_t i = 0;
165  for (FixedTransformsMap::const_iterator it = transforms_.begin(); it != transforms_.end(); ++it, ++i)
166  {
167  transforms[i].child_frame_id = target_frame_;
168  transforms[i].header.frame_id = it->first;
169  tf::transformEigenToMsg(it->second, transforms[i].transform);
170  }
171 }
void copyTransforms(std::vector< geometry_msgs::TransformStamped > &transforms) const
Get a vector of all the transforms as ROS messages.
Definition: transforms.cpp:161
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
Definition: transforms.cpp:74
std::string target_frame_
Definition: transforms.h:198
void transformEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Transform &m)
virtual bool canTransform(const std::string &from_frame) const
Check whether data can be transformed from a particular frame.
Definition: transforms.cpp:115
static bool sameFrame(const std::string &frame1, const std::string &frame2)
Check if two frames end up being the same once the missing / are added as prefix (if they are missing...
Definition: transforms.cpp:59
virtual const Eigen::Affine3d & getTransform(const std::string &from_frame) const
Get transform for from_frame (w.r.t target frame)
Definition: transforms.cpp:97
Transforms(const std::string &target_frame)
Construct a transform list.
Definition: transforms.cpp:42
void setTransform(const Eigen::Affine3d &t, const std::string &from_frame)
Set a transform in the transform tree (adding it if necessary)
Definition: transforms.cpp:124
void setTransforms(const std::vector< geometry_msgs::TransformStamped > &transforms)
Set a transform in the transform tree (adding it if necessary)
Definition: transforms.cpp:155
void setAllTransforms(const FixedTransformsMap &transforms)
Set all the transforms: a map from string names of frames to corresponding Eigen::Affine3d (w...
Definition: transforms.cpp:84
FixedTransformsMap transforms_
Definition: transforms.h:199
const FixedTransformsMap & getAllTransforms() const
Return all the transforms.
Definition: transforms.cpp:79
virtual bool isFixedFrame(const std::string &frame) const
Check whether a frame stays constant as the state of the robot model changes. This is true for any tr...
Definition: transforms.cpp:89
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
virtual ~Transforms()
Destructor.
Definition: transforms.cpp:70
std::map< std::string, Eigen::Affine3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Affine3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:56


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44