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 
38 #include <tf2_eigen/tf2_eigen.h>
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  transforms_map_[target_frame_] = Eigen::Isometry3d::Identity();
54  }
55 }
56 
57 bool Transforms::sameFrame(const std::string& frame1, const std::string& frame2)
58 {
59  if (frame1.empty() || frame2.empty())
60  return false;
61  return frame1 == frame2;
62 }
63 
64 Transforms::~Transforms() = default;
65 
66 const std::string& Transforms::getTargetFrame() const
67 {
68  return target_frame_;
69 }
70 
72 {
73  return transforms_map_;
74 }
75 
77 {
78  transforms_map_ = transforms;
79 }
80 
81 bool Transforms::isFixedFrame(const std::string& frame) const
82 {
83  if (frame.empty())
84  return false;
85  else
86  return transforms_map_.find(frame) != transforms_map_.end();
87 }
88 
89 const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame) const
90 {
91  if (!from_frame.empty())
92  {
93  FixedTransformsMap::const_iterator it = transforms_map_.find(from_frame);
94  if (it != transforms_map_.end())
95  return it->second;
96  // If no transform found in map, return identity
97  }
98 
99  ROS_ERROR_NAMED("transforms", "Unable to transform from frame '%s' to frame '%s'. Returning identity.",
100  from_frame.c_str(), target_frame_.c_str());
101 
102  // return identity
103  static const Eigen::Isometry3d IDENTITY = Eigen::Isometry3d::Identity();
104  return IDENTITY;
105 }
106 
107 bool Transforms::canTransform(const std::string& from_frame) const
108 {
109  if (from_frame.empty())
110  return false;
111  else
112  return transforms_map_.find(from_frame) != transforms_map_.end();
113 }
114 
115 void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame)
116 {
117  if (from_frame.empty())
118  ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name");
119  else
120  transforms_map_[from_frame] = t;
121 }
122 
123 void Transforms::setTransform(const geometry_msgs::TransformStamped& transform)
124 {
125  if (sameFrame(transform.child_frame_id, target_frame_))
126  {
127  Eigen::Isometry3d t = tf2::transformToEigen(transform.transform);
128  setTransform(t, transform.header.frame_id);
129  }
130  else
131  {
132  ROS_ERROR_NAMED("transforms", "Given transform is to frame '%s', but frame '%s' was expected.",
133  transform.child_frame_id.c_str(), target_frame_.c_str());
134  }
135 }
136 
137 void Transforms::setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms)
138 {
139  for (std::size_t i = 0; i < transforms.size(); ++i)
140  setTransform(transforms[i]);
141 }
142 
143 void Transforms::copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const
144 {
145  transforms.resize(transforms_map_.size());
146  std::size_t i = 0;
147  for (FixedTransformsMap::const_iterator it = transforms_map_.begin(); it != transforms_map_.end(); ++it, ++i)
148  {
149  transforms[i] = tf2::eigenToTransform(it->second);
150  transforms[i].child_frame_id = target_frame_;
151  transforms[i].header.frame_id = it->first;
152  }
153 }
154 
155 } // end of namespace core
156 } // end of namespace moveit
FixedTransformsMap transforms_map_
Definition: transforms.h:198
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
std::string target_frame_
Definition: transforms.h:197
virtual const Eigen::Isometry3d & getTransform(const std::string &from_frame) const
Get transform for from_frame (w.r.t target frame)
Definition: transforms.cpp:89
const FixedTransformsMap & getAllTransforms() const
Return all the transforms.
Definition: transforms.cpp:71
virtual bool canTransform(const std::string &from_frame) const
Check whether data can be transformed from a particular frame.
Definition: transforms.cpp:107
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:57
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:81
geometry_msgs::TransformStamped t
Transforms(const std::string &target_frame)
Construct a transform list.
Definition: transforms.cpp:46
void copyTransforms(std::vector< geometry_msgs::TransformStamped > &transforms) const
Get a vector of all the transforms as ROS messages.
Definition: transforms.cpp:143
void setTransforms(const std::vector< geometry_msgs::TransformStamped > &transforms)
Set a transform in the transform tree (adding it if necessary)
Definition: transforms.cpp:137
void setAllTransforms(const FixedTransformsMap &transforms)
Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w...
Definition: transforms.cpp:76
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
Definition: transforms.cpp:66
#define ROS_ERROR_NAMED(name,...)
virtual ~Transforms()
Destructor.
std::map< std::string, Eigen::Isometry3d, std::less< std::string >, Eigen::aligned_allocator< std::pair< const std::string, Eigen::Isometry3d > > > FixedTransformsMap
Map frame names to the transformation matrix that can transform objects from the frame name to the pl...
Definition: transforms.h:55
Main namespace for MoveIt!
void setTransform(const Eigen::Isometry3d &t, const std::string &from_frame)
Set a transform in the transform tree (adding it if necessary)
Definition: transforms.cpp:115
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Sep 8 2020 04:12:46