tf2_kdl.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
32 #ifndef TF2_KDL_H
33 #define TF2_KDL_H
34 
35 #include <tf2/convert.h>
36 #include <kdl/frames.hpp>
37 #include <geometry_msgs/PointStamped.h>
38 #include <geometry_msgs/TwistStamped.h>
39 #include <geometry_msgs/WrenchStamped.h>
40 #include <geometry_msgs/PoseStamped.h>
41 #include <geometry_msgs/Pose.h>
42 
43 
44 namespace tf2
45 {
50 inline
51 KDL::Frame transformToKDL(const geometry_msgs::TransformStamped& t)
52  {
53  return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y,
54  t.transform.rotation.z, t.transform.rotation.w),
55  KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
56  }
57 
62 inline
63 geometry_msgs::TransformStamped kdlToTransform(const KDL::Frame& k)
64 {
65  geometry_msgs::TransformStamped t;
66  t.transform.translation.x = k.p.x();
67  t.transform.translation.y = k.p.y();
68  t.transform.translation.z = k.p.z();
69  k.M.GetQuaternion(t.transform.rotation.x, t.transform.rotation.y, t.transform.rotation.z, t.transform.rotation.w);
70  return t;
71 }
72 
73 // ---------------------
74 // Vector
75 // ---------------------
82 template <>
83 inline
84  void doTransform(const tf2::Stamped<KDL::Vector>& t_in, tf2::Stamped<KDL::Vector>& t_out, const geometry_msgs::TransformStamped& transform)
85  {
86  t_out = tf2::Stamped<KDL::Vector>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
87  }
88 
94 inline
95 geometry_msgs::PointStamped toMsg(const tf2::Stamped<KDL::Vector>& in)
96 {
97  geometry_msgs::PointStamped msg;
98  msg.header.stamp = in.stamp_;
99  msg.header.frame_id = in.frame_id_;
100  msg.point.x = in[0];
101  msg.point.y = in[1];
102  msg.point.z = in[2];
103  return msg;
104 }
105 
111 inline
112 void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<KDL::Vector>& out)
113 {
114  out.stamp_ = msg.header.stamp;
115  out.frame_id_ = msg.header.frame_id;
116  out[0] = msg.point.x;
117  out[1] = msg.point.y;
118  out[2] = msg.point.z;
119 }
120 
121 // ---------------------
122 // Twist
123 // ---------------------
130 template <>
131 inline
132  void doTransform(const tf2::Stamped<KDL::Twist>& t_in, tf2::Stamped<KDL::Twist>& t_out, const geometry_msgs::TransformStamped& transform)
133  {
134  t_out = tf2::Stamped<KDL::Twist>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
135  }
136 
142 inline
143 geometry_msgs::TwistStamped toMsg(const tf2::Stamped<KDL::Twist>& in)
144 {
145  geometry_msgs::TwistStamped msg;
146  msg.header.stamp = in.stamp_;
147  msg.header.frame_id = in.frame_id_;
148  msg.twist.linear.x = in.vel[0];
149  msg.twist.linear.y = in.vel[1];
150  msg.twist.linear.z = in.vel[2];
151  msg.twist.angular.x = in.rot[0];
152  msg.twist.angular.y = in.rot[1];
153  msg.twist.angular.z = in.rot[2];
154  return msg;
155 }
156 
162 inline
163 void fromMsg(const geometry_msgs::TwistStamped& msg, tf2::Stamped<KDL::Twist>& out)
164 {
165  out.stamp_ = msg.header.stamp;
166  out.frame_id_ = msg.header.frame_id;
167  out.vel[0] = msg.twist.linear.x;
168  out.vel[1] = msg.twist.linear.y;
169  out.vel[2] = msg.twist.linear.z;
170  out.rot[0] = msg.twist.angular.x;
171  out.rot[1] = msg.twist.angular.y;
172  out.rot[2] = msg.twist.angular.z;
173 }
174 
175 
176 // ---------------------
177 // Wrench
178 // ---------------------
185 template <>
186 inline
187  void doTransform(const tf2::Stamped<KDL::Wrench>& t_in, tf2::Stamped<KDL::Wrench>& t_out, const geometry_msgs::TransformStamped& transform)
188  {
189  t_out = tf2::Stamped<KDL::Wrench>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
190  }
191 
197 inline
198 geometry_msgs::WrenchStamped toMsg(const tf2::Stamped<KDL::Wrench>& in)
199 {
200  geometry_msgs::WrenchStamped msg;
201  msg.header.stamp = in.stamp_;
202  msg.header.frame_id = in.frame_id_;
203  msg.wrench.force.x = in.force[0];
204  msg.wrench.force.y = in.force[1];
205  msg.wrench.force.z = in.force[2];
206  msg.wrench.torque.x = in.torque[0];
207  msg.wrench.torque.y = in.torque[1];
208  msg.wrench.torque.z = in.torque[2];
209  return msg;
210 }
211 
217 inline
218 void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped<KDL::Wrench>& out)
219 {
220  out.stamp_ = msg.header.stamp;
221  out.frame_id_ = msg.header.frame_id;
222  out.force[0] = msg.wrench.force.x;
223  out.force[1] = msg.wrench.force.y;
224  out.force[2] = msg.wrench.force.z;
225  out.torque[0] = msg.wrench.torque.x;
226  out.torque[1] = msg.wrench.torque.y;
227  out.torque[2] = msg.wrench.torque.z;
228 }
229 
230 
231 
232 
233 // ---------------------
234 // Frame
235 // ---------------------
242 template <>
243 inline
244  void doTransform(const tf2::Stamped<KDL::Frame>& t_in, tf2::Stamped<KDL::Frame>& t_out, const geometry_msgs::TransformStamped& transform)
245  {
246  t_out = tf2::Stamped<KDL::Frame>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
247  }
248 
254 inline
255 geometry_msgs::Pose toMsg(const KDL::Frame& in)
256 {
257  geometry_msgs::Pose msg;
258  msg.position.x = in.p[0];
259  msg.position.y = in.p[1];
260  msg.position.z = in.p[2];
261  in.M.GetQuaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w);
262  return msg;
263 }
264 
270 inline
271 void fromMsg(const geometry_msgs::Pose& msg, KDL::Frame& out)
272 {
273  out.p[0] = msg.position.x;
274  out.p[1] = msg.position.y;
275  out.p[2] = msg.position.z;
276  out.M = KDL::Rotation::Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w);
277 }
278 
284 inline
285 geometry_msgs::PoseStamped toMsg(const tf2::Stamped<KDL::Frame>& in)
286 {
287  geometry_msgs::PoseStamped msg;
288  msg.header.stamp = in.stamp_;
289  msg.header.frame_id = in.frame_id_;
290  msg.pose = toMsg(static_cast<const KDL::Frame&>(in));
291  return msg;
292 }
293 
299 inline
300 void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<KDL::Frame>& out)
301 {
302  out.stamp_ = msg.header.stamp;
303  out.frame_id_ = msg.header.frame_id;
304  fromMsg(msg.pose, static_cast<KDL::Frame&>(out));
305 }
306 
307 } // namespace
308 
309 #endif // TF2_KDL_H
static Rotation Quaternion(double x, double y, double z, double w)
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
double z() const
Rotation M
void GetQuaternion(double &x, double &y, double &z, double &w) const
KDL::Frame transformToKDL(const geometry_msgs::TransformStamped &t)
Convert a timestamped transform to the equivalent KDL data type.
Definition: tf2_kdl.h:51
double y() const
ros::Time stamp_
double x() const
void fromMsg(const A &, B &b)
B toMsg(const A &a)
std::string frame_id_
geometry_msgs::TransformStamped kdlToTransform(const KDL::Frame &k)
Convert an KDL Frame to the equivalent geometry_msgs message type.
Definition: tf2_kdl.h:63


tf2_kdl
Author(s): Wim Meeussen
autogenerated on Fri Jun 7 2019 21:45:52