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 <tf2_eigen/tf2_eigen.h>
40 #include <boost/algorithm/string/trim.hpp>
41 #include <ros/console.h>
42 
43 namespace moveit
44 {
45 namespace core
46 {
47 Transforms::Transforms(const std::string& target_frame) : target_frame_(target_frame)
48 {
49  boost::trim(target_frame_);
50  if (target_frame_.empty())
51  ROS_ERROR_NAMED("transforms", "The target frame for MoveIt Transforms cannot be empty.");
52  else
53  {
54  transforms_map_[target_frame_] = Eigen::Isometry3d::Identity();
55  }
56 }
57 
58 bool Transforms::sameFrame(const std::string& frame1, const std::string& frame2)
59 {
60  if (frame1.empty() || frame2.empty())
61  return false;
62  return frame1 == frame2;
63 }
64 
65 Transforms::~Transforms() = default;
66 
67 const std::string& Transforms::getTargetFrame() const
68 {
69  return target_frame_;
70 }
71 
72 const FixedTransformsMap& Transforms::getAllTransforms() const
73 {
74  return transforms_map_;
75 }
76 
77 void Transforms::setAllTransforms(const FixedTransformsMap& transforms)
78 {
79  for (const auto& t : transforms)
80  {
81  ASSERT_ISOMETRY(t.second) // unsanitized input, could contain a non-isometry
82  }
83  transforms_map_ = transforms;
84 }
85 
86 bool Transforms::isFixedFrame(const std::string& frame) const
87 {
88  if (frame.empty())
89  return false;
90  else
91  return transforms_map_.find(frame) != transforms_map_.end();
92 }
93 
94 const Eigen::Isometry3d& Transforms::getTransform(const std::string& from_frame) const
95 {
96  if (!from_frame.empty())
97  {
98  FixedTransformsMap::const_iterator it = transforms_map_.find(from_frame);
99  if (it != transforms_map_.end())
100  return it->second;
101  // If no transform found in map, return identity
102  }
103 
104  ROS_ERROR_NAMED("transforms", "Unable to transform from frame '%s' to frame '%s'. Returning identity.",
105  from_frame.c_str(), target_frame_.c_str());
106 
107  // return identity
108  static const Eigen::Isometry3d IDENTITY = Eigen::Isometry3d::Identity();
109  return IDENTITY;
110 }
111 
112 bool Transforms::canTransform(const std::string& from_frame) const
113 {
114  if (from_frame.empty())
115  return false;
116  else
117  return transforms_map_.find(from_frame) != transforms_map_.end();
118 }
119 
120 void Transforms::setTransform(const Eigen::Isometry3d& t, const std::string& from_frame)
121 {
122  ASSERT_ISOMETRY(t) // unsanitized input, could contain a non-isometry
123  if (from_frame.empty())
124  ROS_ERROR_NAMED("transforms", "Cannot record transform with empty name");
125  else
126  transforms_map_[from_frame] = t;
127 }
128 
129 void Transforms::setTransform(const geometry_msgs::TransformStamped& transform)
130 {
131  if (sameFrame(transform.child_frame_id, target_frame_))
132  {
133  Eigen::Isometry3d t = tf2::transformToEigen(transform.transform);
134  setTransform(t, transform.header.frame_id);
135  }
136  else
137  {
138  ROS_ERROR_NAMED("transforms", "Given transform is to frame '%s', but frame '%s' was expected.",
139  transform.child_frame_id.c_str(), target_frame_.c_str());
140  }
141 }
142 
143 void Transforms::setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms)
144 {
145  for (const geometry_msgs::TransformStamped& transform : transforms)
146  setTransform(transform);
147 }
148 
149 void Transforms::copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const
150 {
151  transforms.resize(transforms_map_.size());
152  std::size_t i = 0;
153  for (FixedTransformsMap::const_iterator it = transforms_map_.begin(); it != transforms_map_.end(); ++it, ++i)
154  {
155  transforms[i] = tf2::eigenToTransform(it->second);
156  transforms[i].child_frame_id = target_frame_;
157  transforms[i].header.frame_id = it->first;
158  }
159 }
160 
161 } // end of namespace core
162 } // end of namespace moveit
tf2_eigen.h
check_isometry.h
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
tf2::eigenToTransform
geometry_msgs::TransformStamped eigenToTransform(const Eigen::Affine3d &T)
transforms.h
console.h
ASSERT_ISOMETRY
#define ASSERT_ISOMETRY(transform)
moveit::core::FixedTransformsMap
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:117
tf2::transformToEigen
Eigen::Isometry3d transformToEigen(const geometry_msgs::Transform &t)
moveit
Main namespace for MoveIt.
Definition: background_processing.h:46
moveit::core::Transforms::Transforms
Transforms(const std::string &target_frame)
Construct a transform list.
Definition: transforms.cpp:111
t
geometry_msgs::TransformStamped t


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Nov 24 2020 03:26:40