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  // convert message manually to ensure correct normalization for double (error < 1e-12)
134  // tf2 only enforces float normalization (error < 1e-5)
135  const auto& trans = transform.transform.translation;
136  const auto& rot = transform.transform.rotation;
137  Eigen::Translation3d translation(trans.x, trans.y, trans.z);
138  Eigen::Quaterniond rotation(rot.w, rot.x, rot.y, rot.z);
139  rotation.normalize();
140 
141  setTransform(translation * rotation, transform.header.frame_id);
142  }
143  else
144  {
145  ROS_ERROR_NAMED("transforms", "Given transform is to frame '%s', but frame '%s' was expected.",
146  transform.child_frame_id.c_str(), target_frame_.c_str());
147  }
148 }
149 
150 void Transforms::setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms)
151 {
152  for (const geometry_msgs::TransformStamped& transform : transforms)
153  setTransform(transform);
154 }
155 
156 void Transforms::copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const
157 {
158  transforms.resize(transforms_map_.size());
159  std::size_t i = 0;
160  for (FixedTransformsMap::const_iterator it = transforms_map_.begin(); it != transforms_map_.end(); ++it, ++i)
161  {
162  transforms[i] = tf2::eigenToTransform(it->second);
163  transforms[i].child_frame_id = target_frame_;
164  transforms[i].header.frame_id = it->first;
165  }
166 }
167 
168 } // end of namespace core
169 } // 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
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 Sun Oct 24 2021 02:23:52