transforms.h
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 
37 #ifndef MOVEIT_TRANSFORMS_
38 #define MOVEIT_TRANSFORMS_
39 
40 #include <geometry_msgs/TransformStamped.h>
41 #include <geometry_msgs/Pose.h>
42 #include <Eigen/Geometry>
43 #include <boost/noncopyable.hpp>
45 
46 namespace moveit
47 {
48 namespace core
49 {
50 MOVEIT_CLASS_FORWARD(Transforms);
51 
54 typedef std::map<std::string, Eigen::Affine3d, std::less<std::string>,
55  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Affine3d> > >
57 
61 class Transforms : private boost::noncopyable
62 {
63 public:
67  Transforms(const std::string& target_frame);
68 
72  virtual ~Transforms();
73 
75  static bool sameFrame(const std::string& frame1, const std::string& frame2);
76 
81  const std::string& getTargetFrame() const;
82 
92  const FixedTransformsMap& getAllTransforms() const;
93 
98  void copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const;
99 
105  void setTransform(const Eigen::Affine3d& t, const std::string& from_frame);
106 
111  void setTransform(const geometry_msgs::TransformStamped& transform);
112 
117  void setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms);
118 
123  void setAllTransforms(const FixedTransformsMap& transforms);
124 
139  void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const
140  {
141  v_out = getTransform(from_frame).linear() * v_in;
142  }
143 
150  void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in,
151  Eigen::Quaterniond& q_out) const
152  {
153  q_out = getTransform(from_frame).linear() * q_in;
154  }
155 
162  void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const
163  {
164  m_out = getTransform(from_frame).linear() * m_in;
165  }
166 
173  void transformPose(const std::string& from_frame, const Eigen::Affine3d& t_in, Eigen::Affine3d& t_out) const
174  {
175  t_out = getTransform(from_frame) * t_in;
176  }
182  virtual bool canTransform(const std::string& from_frame) const;
183 
188  virtual bool isFixedFrame(const std::string& frame) const;
189 
195  virtual const Eigen::Affine3d& getTransform(const std::string& from_frame) const;
196 
197 protected:
198  std::string target_frame_;
200 };
201 }
202 }
203 
204 #endif
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
std::string target_frame_
Definition: transforms.h:198
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
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
Definition: transforms.h:61
void transformQuaternion(const std::string &from_frame, const Eigen::Quaterniond &q_in, Eigen::Quaterniond &q_out) const
Transform a quaternion in from_frame to the target_frame.
Definition: transforms.h:150
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
MOVEIT_CLASS_FORWARD(RobotModel)
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 transformPose(const std::string &from_frame, const Eigen::Affine3d &t_in, Eigen::Affine3d &t_out) const
Transform a pose in from_frame to the target_frame.
Definition: transforms.h:173
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
void transformVector3(const std::string &from_frame, const Eigen::Vector3d &v_in, Eigen::Vector3d &v_out) const
Transform a vector in from_frame to the target_frame Note: assumes that v_in and v_out are "free" vec...
Definition: transforms.h:139
void transformRotationMatrix(const std::string &from_frame, const Eigen::Matrix3d &m_in, Eigen::Matrix3d &m_out) const
Transform a rotation matrix in from_frame to the target_frame.
Definition: transforms.h:162
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
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 Wed Jul 10 2019 04:03:05