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 #pragma once
38 
39 #include <geometry_msgs/TransformStamped.h>
40 #include <Eigen/Geometry>
41 #include <boost/noncopyable.hpp>
43 
44 namespace moveit
45 {
46 namespace core
47 {
48 MOVEIT_CLASS_FORWARD(Transforms); // Defines TransformsPtr, ConstPtr, WeakPtr... etc
49 
52 using FixedTransformsMap = std::map<std::string, Eigen::Isometry3d, std::less<std::string>,
53  Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d> > >;
54 
58 class Transforms : private boost::noncopyable
59 {
60 public:
64  Transforms(const std::string& target_frame);
65 
69  virtual ~Transforms();
70 
72  static bool sameFrame(const std::string& frame1, const std::string& frame2);
73 
78  const std::string& getTargetFrame() const;
79 
90  const FixedTransformsMap& getAllTransforms() const;
91 
96  void copyTransforms(std::vector<geometry_msgs::TransformStamped>& transforms) const;
97 
103  void setTransform(const Eigen::Isometry3d& t, const std::string& from_frame);
104 
109  void setTransform(const geometry_msgs::TransformStamped& transform);
110 
115  void setTransforms(const std::vector<geometry_msgs::TransformStamped>& transforms);
116 
121  void setAllTransforms(const FixedTransformsMap& transforms);
122 
137  void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const
138  {
139  // getTransform() returns a valid isometry by contract
140  v_out = getTransform(from_frame).linear() * v_in;
141  }
142 
149  void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in,
150  Eigen::Quaterniond& q_out) const
151  {
152  // getTransform() returns a valid isometry by contract
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  // getTransform() returns a valid isometry by contract
165  m_out = getTransform(from_frame).linear() * m_in;
166  }
167 
174  void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const
175  {
176  // getTransform() returns a valid isometry by contract
177  t_out = getTransform(from_frame) * t_in;
178  }
184  virtual bool canTransform(const std::string& from_frame) const;
185 
190  virtual bool isFixedFrame(const std::string& frame) const;
191 
197  virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const;
198 
199 protected:
200  std::string target_frame_;
202 };
203 } // namespace core
204 } // namespace moveit
moveit::core::Transforms::target_frame_
std::string target_frame_
Definition: transforms.h:264
moveit::core::Transforms::transformRotationMatrix
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:226
moveit::core::Transforms::transformPose
void transformPose(const std::string &from_frame, const Eigen::Isometry3d &t_in, Eigen::Isometry3d &t_out) const
Transform a pose in from_frame to the target_frame.
Definition: transforms.h:238
moveit::core::Transforms::setTransforms
void setTransforms(const std::vector< geometry_msgs::TransformStamped > &transforms)
Set a transform in the transform tree (adding it if necessary)
Definition: transforms.cpp:207
moveit::core::Transforms::transformVector3
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:201
moveit::core::Transforms::getTargetFrame
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
Definition: transforms.cpp:131
moveit::core::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(RobotModel)
class_forward.h
moveit::core::Transforms::canTransform
virtual bool canTransform(const std::string &from_frame) const
Check whether data can be transformed from a particular frame.
Definition: transforms.cpp:176
moveit::core::Transforms::~Transforms
virtual ~Transforms()
Destructor.
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::core::Transforms::setAllTransforms
void setAllTransforms(const FixedTransformsMap &transforms)
Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w....
Definition: transforms.cpp:141
moveit::core::Transforms::sameFrame
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:122
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
moveit::core::Transforms::getTransform
virtual const Eigen::Isometry3d & getTransform(const std::string &from_frame) const
Get transform for from_frame (w.r.t target frame)
Definition: transforms.cpp:158
moveit::core::Transforms::copyTransforms
void copyTransforms(std::vector< geometry_msgs::TransformStamped > &transforms) const
Get a vector of all the transforms as ROS messages.
Definition: transforms.cpp:213
moveit::core::Transforms::transforms_map_
FixedTransformsMap transforms_map_
Definition: transforms.h:265
moveit::core::Transforms::setTransform
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:184
moveit::core::Transforms::getAllTransforms
const FixedTransformsMap & getAllTransforms() const
Return all the transforms.
Definition: transforms.cpp:136
moveit::core::Transforms::isFixedFrame
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:150
moveit::core::Transforms::transformQuaternion
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:213
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Oct 21 2020 03:28:04