moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
transforms.hpp
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/msg/transform_stamped.hpp>
40#include <Eigen/Geometry>
42#include <map>
43
44namespace moveit
45{
46namespace core
47{
48MOVEIT_CLASS_FORWARD(Transforms); // Defines TransformsPtr, ConstPtr, WeakPtr... etc
49
52using FixedTransformsMap = std::map<std::string, Eigen::Isometry3d, std::less<std::string>,
53 Eigen::aligned_allocator<std::pair<const std::string, Eigen::Isometry3d> > >;
54
59{
60public:
64 Transforms(const Transforms&) = delete;
65
69 Transforms& operator=(const Transforms&) = delete;
70
74 Transforms(const std::string& target_frame);
75
79 virtual ~Transforms();
80
82 static bool sameFrame(const std::string& frame1, const std::string& frame2);
83
88 const std::string& getTargetFrame() const;
89
101
106 void copyTransforms(std::vector<geometry_msgs::msg::TransformStamped>& transforms) const;
107
113 void setTransform(const Eigen::Isometry3d& t, const std::string& from_frame);
114
119 void setTransform(const geometry_msgs::msg::TransformStamped& transform);
120
125 void setTransforms(const std::vector<geometry_msgs::msg::TransformStamped>& transforms);
126
131 void setAllTransforms(const FixedTransformsMap& transforms);
132
147 void transformVector3(const std::string& from_frame, const Eigen::Vector3d& v_in, Eigen::Vector3d& v_out) const
148 {
149 // getTransform() returns a valid isometry by contract
150 v_out = getTransform(from_frame).linear() * v_in;
151 }
152
159 void transformQuaternion(const std::string& from_frame, const Eigen::Quaterniond& q_in,
160 Eigen::Quaterniond& q_out) const
161 {
162 // getTransform() returns a valid isometry by contract
163 q_out = getTransform(from_frame).linear() * q_in;
164 }
165
172 void transformRotationMatrix(const std::string& from_frame, const Eigen::Matrix3d& m_in, Eigen::Matrix3d& m_out) const
173 {
174 // getTransform() returns a valid isometry by contract
175 m_out = getTransform(from_frame).linear() * m_in;
176 }
177
184 void transformPose(const std::string& from_frame, const Eigen::Isometry3d& t_in, Eigen::Isometry3d& t_out) const
185 {
186 // getTransform() returns a valid isometry by contract
187 t_out = getTransform(from_frame) * t_in;
188 }
194 virtual bool canTransform(const std::string& from_frame) const;
195
200 virtual bool isFixedFrame(const std::string& frame) const;
201
207 virtual const Eigen::Isometry3d& getTransform(const std::string& from_frame) const;
208
209protected:
210 std::string target_frame_;
212};
213} // namespace core
214} // namespace moveit
#define MOVEIT_CLASS_FORWARD(C)
Provides an implementation of a snapshot of a transform tree that can be easily queried for transform...
void setTransforms(const std::vector< geometry_msgs::msg::TransformStamped > &transforms)
Set a transform in the transform tree (adding it if necessary)
const std::string & getTargetFrame() const
Get the planning frame corresponding to this set of transforms.
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...
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.
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.
Transforms(const Transforms &)=delete
Transforms cannot be copy-constructed.
virtual ~Transforms()
Destructor.
void setTransform(const Eigen::Isometry3d &t, const std::string &from_frame)
Set a transform in the transform tree (adding it if necessary)
const FixedTransformsMap & getAllTransforms() const
Return all the transforms.
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.
virtual const Eigen::Isometry3d & getTransform(const std::string &from_frame) const
Get transform for from_frame (w.r.t target frame)
Transforms & operator=(const Transforms &)=delete
Transforms cannot be copy-assigned.
void copyTransforms(std::vector< geometry_msgs::msg::TransformStamped > &transforms) const
Get a vector of all the transforms as ROS messages.
void setAllTransforms(const FixedTransformsMap &transforms)
Set all the transforms: a map from string names of frames to corresponding Eigen::Isometry3d (w....
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...
FixedTransformsMap transforms_map_
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...
virtual bool canTransform(const std::string &from_frame) const
Check whether data can be transformed from a particular frame.
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...
Main namespace for MoveIt.