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 <ros/console.h>
41 
42 namespace moveit
43 {
44 namespace core
45 {
46 Transforms::Transforms(const std::string& target_frame) : target_frame_(target_frame)
47 {
48  boost::trim(target_frame_);
49  if (target_frame_.empty())
50  ROS_ERROR_NAMED("transforms", "The target frame for MoveIt Transforms cannot be empty.");
51  else
52  {
53  if (target_frame_[0] != '/')
54  {
55  ROS_WARN_NAMED("transforms",
56  "Frame '%s' specified as target frame for MoveIt Transforms. Assuming '/%s' instead.",
57  target_frame_.c_str(), target_frame_.c_str());
59  }
60  transforms_[target_frame_] = Eigen::Affine3d::Identity();
61  }
62 }
63 
64 bool Transforms::sameFrame(const std::string& frame1, const std::string& frame2)
65 {
66  if (frame1.empty() || frame2.empty())
67  return false;
68  if (frame1[0] != '/')
69  return sameFrame('/' + frame1, frame2);
70  if (frame2[0] != '/')
71  return sameFrame(frame1, '/' + frame2);
72  return frame1 == frame2;
73 }
74 
75 Transforms::~Transforms() = default;
76 
77 const std::string& Transforms::getTargetFrame() const
78 {
79  return target_frame_;
80 }
81 
83 {
84  return transforms_;
85 }
86 
88 {
89  transforms_ = transforms;
90 }
91 
92 bool Transforms::isFixedFrame(const std::string& frame) const
93 {
94  if (frame.empty())
95  return false;
96  else
97  return (frame[0] == '/' ? transforms_.find(frame) : transforms_.find('/' + frame)) != transforms_.end();
98 }
99 
100 const Eigen::Affine3d& Transforms::getTransform(const std::string& from_frame) const
101 {
102  if (!from_frame.empty())
103  {
104  FixedTransformsMap::const_iterator it =
105  (from_frame[0] == '/' ? transforms_.find(from_frame) : transforms_.find('/' + from_frame));
106  if (it != transforms_.end())
107  return it->second;
108  }
109 
110  ROS_ERROR_NAMED("transforms", "Unable to transform from frame '%s' to frame '%s'. Returning identity.",
111  from_frame.c_str(), target_frame_.c_str());
112 
113  // return identity
114  static const Eigen::Affine3d identity = Eigen::Affine3d::Identity();
115  return identity;
116 }
117 
118 bool Transforms::canTransform(const std::string& from_frame) const
119 {
120  if (from_frame.empty())
121  return false;
122  else
123  return (from_frame[0] == '/' ? transforms_.find(from_frame) : transforms_.find('/' + from_frame)) !=
124  transforms_.end();
125 }
126 
127 void Transforms::setTransform(const Eigen::Affine3d& t, const std::string& from_frame)
128 {
129  if (from_frame.empty())
130  ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name");
131  else
132  {
133  if (from_frame[0] != '/')
134  {
135  ROS_WARN_NAMED("transforms", "Transform specified for frame '%s'. Assuming '/%s' instead", from_frame.c_str(),
136  from_frame.c_str());
137  transforms_['/' + from_frame] = t;
138  }
139  else
140  transforms_[from_frame] = t;
141  }
142 }
143 
144 void Transforms::setTransform(const geometry_msgs::TransformStamped& transform)
145 {
146  if (sameFrame(transform.child_frame_id, target_frame_))
147  {
148  Eigen::Affine3d t;
149  tf::transformMsgToEigen(transform.transform, t);
150  setTransform(t, transform.header.frame_id);
151  }
152  else
153  {
154  ROS_ERROR_NAMED("transforms", "Given transform is to frame '%s', but frame '%s' was expected.",
155  transform.child_frame_id.c_str(), target_frame_.c_str());
156  }
157 }
158 
159 void Transforms::setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms)
160 {
161  for (std::size_t i = 0; i < transforms.size(); ++i)
162  setTransform(transforms[i]);
163 }
164 
165 void Transforms::copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const
166 {
167  transforms.resize(transforms_.size());
168  std::size_t i = 0;
169  for (FixedTransformsMap::const_iterator it = transforms_.begin(); it != transforms_.end(); ++it, ++i)
170  {
171  transforms[i].child_frame_id = target_frame_;
172  transforms[i].header.frame_id = it->first;
173  tf::transformEigenToMsg(it->second, transforms[i].transform);
174  }
175 }
176 
177 } // end of namespace core
178 } // end of namespace moveit
void copyTransforms(std::vector< geometry_msgs::TransformStamped > &transforms) const
Get a vector of all the transforms as ROS messages.
Definition: transforms.cpp:165
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
Definition: transforms.cpp:77
#define ROS_WARN_NAMED(name,...)
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:118
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:64
virtual const Eigen::Affine3d & getTransform(const std::string &from_frame) const
Get transform for from_frame (w.r.t target frame)
Definition: transforms.cpp:100
Transforms(const std::string &target_frame)
Construct a transform list.
Definition: transforms.cpp:46
const FixedTransformsMap & getAllTransforms() const
Return all the transforms.
Definition: transforms.cpp:82
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:127
void setTransforms(const std::vector< geometry_msgs::TransformStamped > &transforms)
Set a transform in the transform tree (adding it if necessary)
Definition: transforms.cpp:159
void setAllTransforms(const FixedTransformsMap &transforms)
Set all the transforms: a map from string names of frames to corresponding Eigen::Affine3d (w...
Definition: transforms.cpp:87
FixedTransformsMap transforms_
Definition: transforms.h:199
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:92
void transformMsgToEigen(const geometry_msgs::Transform &m, Eigen::Affine3d &e)
#define ROS_ERROR_NAMED(name,...)
virtual ~Transforms()
Destructor.
Main namespace for MoveIt!
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 Tue Jun 12 2018 02:47:31