tf2_geometry_msgs  master
tf2_geometry_msgs
tf2_geometry_msgs.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_GEOMETRY_MSGS_H
33 #define TF2_GEOMETRY_MSGS_H
34 
35 #include <tf2/convert.h>
36 #include <tf2/LinearMath/Quaternion.h>
37 #include <tf2/LinearMath/Transform.h>
38 #include <tf2_ros/buffer_interface.h>
39 #include <geometry_msgs/msg/point_stamped.hpp>
40 #include <geometry_msgs/msg/quaternion_stamped.hpp>
41 #include <geometry_msgs/msg/transform_stamped.hpp>
42 #include <geometry_msgs/msg/vector3_stamped.hpp>
43 #include <geometry_msgs/msg/pose.hpp>
44 #include <geometry_msgs/msg/pose_stamped.hpp>
45 #include <kdl/frames.hpp>
46 
47 namespace tf2
48 {
49 
54 inline
55 KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped& t)
56  {
57  return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y,
58  t.transform.rotation.z, t.transform.rotation.w),
59  KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
60  }
61 
62 
63 /********************/
65 /********************/
66 
72 template <>
73 inline
74  tf2::TimePoint getTimestamp(const geometry_msgs::msg::Vector3Stamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
75 
81 template <>
82 inline
83  std::string getFrameId(const geometry_msgs::msg::Vector3Stamped& t) {return t.header.frame_id;}
84 
91 template <>
92 inline
93  void doTransform(const geometry_msgs::msg::Vector3Stamped& t_in, geometry_msgs::msg::Vector3Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
94  {
95  KDL::Vector v_out = gmTransformToKDL(transform).M * KDL::Vector(t_in.vector.x, t_in.vector.y, t_in.vector.z);
96  t_out.vector.x = v_out[0];
97  t_out.vector.y = v_out[1];
98  t_out.vector.z = v_out[2];
99  t_out.header.stamp = transform.header.stamp;
100  t_out.header.frame_id = transform.header.frame_id;
101  }
102 
108 inline
109 geometry_msgs::msg::Vector3Stamped toMsg(const geometry_msgs::msg::Vector3Stamped& in)
110 {
111  return in;
112 }
113 
119 inline
120 void fromMsg(const geometry_msgs::msg::Vector3Stamped& msg, geometry_msgs::msg::Vector3Stamped& out)
121 {
122  out = msg;
123 }
124 
125 
126 
127 /******************/
129 /******************/
130 
136 template <>
137 inline
138  tf2::TimePoint getTimestamp(const geometry_msgs::msg::PointStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
139 
145 template <>
146 inline
147  std::string getFrameId(const geometry_msgs::msg::PointStamped& t) {return t.header.frame_id;}
148 
155 template <>
156 inline
157  void doTransform(const geometry_msgs::msg::PointStamped& t_in, geometry_msgs::msg::PointStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
158  {
159  KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.point.x, t_in.point.y, t_in.point.z);
160  t_out.point.x = v_out[0];
161  t_out.point.y = v_out[1];
162  t_out.point.z = v_out[2];
163  t_out.header.stamp = transform.header.stamp;
164  t_out.header.frame_id = transform.header.frame_id;
165  }
166 
172 inline
173 geometry_msgs::msg::PointStamped toMsg(const geometry_msgs::msg::PointStamped& in)
174 {
175  return in;
176 }
177 
183 inline
184 void fromMsg(const geometry_msgs::msg::PointStamped& msg, geometry_msgs::msg::PointStamped& out)
185 {
186  out = msg;
187 }
188 
189 
190 /*****************/
192 /*****************/
193 
199 template <>
200 inline
201  tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
202 
208 template <>
209 inline
210  std::string getFrameId(const geometry_msgs::msg::PoseStamped& t) {return t.header.frame_id;}
211 
218 template <>
219 inline
220  void doTransform(const geometry_msgs::msg::PoseStamped& t_in, geometry_msgs::msg::PoseStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
221  {
222  KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z);
223  KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.orientation.x, t_in.pose.orientation.y, t_in.pose.orientation.z, t_in.pose.orientation.w);
224 
225  KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
226  t_out.pose.position.x = v_out.p[0];
227  t_out.pose.position.y = v_out.p[1];
228  t_out.pose.position.z = v_out.p[2];
229  v_out.M.GetQuaternion(t_out.pose.orientation.x, t_out.pose.orientation.y, t_out.pose.orientation.z, t_out.pose.orientation.w);
230  t_out.header.stamp = transform.header.stamp;
231  t_out.header.frame_id = transform.header.frame_id;
232  }
233 
239 inline
240 geometry_msgs::msg::PoseStamped toMsg(const geometry_msgs::msg::PoseStamped& in)
241 {
242  return in;
243 }
244 
250 inline
251 void fromMsg(const geometry_msgs::msg::PoseStamped& msg, geometry_msgs::msg::PoseStamped& out)
252 {
253  out = msg;
254 }
255 
256 
257 /****************/
259 /****************/
260 
266 inline
267 geometry_msgs::msg::Quaternion toMsg(const tf2::Quaternion& in)
268 {
269  geometry_msgs::msg::Quaternion out;
270  out.w = in.getW();
271  out.x = in.getX();
272  out.y = in.getY();
273  out.z = in.getZ();
274  return out;
275 }
276 
282 inline
283 void fromMsg(const geometry_msgs::msg::Quaternion& in, tf2::Quaternion& out)
284 {
285  // w at the end in the constructor
286  out = tf2::Quaternion(in.x, in.y, in.z, in.w);
287 }
288 
289 
290 /***********************/
292 /***********************/
293 
299 template <>
300 inline
301 tf2::TimePoint getTimestamp(const geometry_msgs::msg::QuaternionStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
302 
308 template <>
309 inline
310 std::string getFrameId(const geometry_msgs::msg::QuaternionStamped& t) {return t.header.frame_id;}
311 
318 template <>
319 inline
320 void doTransform(const geometry_msgs::msg::QuaternionStamped& t_in, geometry_msgs::msg::QuaternionStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
321 {
322  tf2::Quaternion q_out = tf2::Quaternion(transform.transform.rotation.x, transform.transform.rotation.y,
323  transform.transform.rotation.z, transform.transform.rotation.w)*
324  tf2::Quaternion(t_in.quaternion.x, t_in.quaternion.y, t_in.quaternion.z, t_in.quaternion.w);
325  t_out.quaternion = toMsg(q_out);
326  t_out.header.stamp = transform.header.stamp;
327  t_out.header.frame_id = transform.header.frame_id;
328 }
329 
335 inline
336 geometry_msgs::msg::QuaternionStamped toMsg(const geometry_msgs::msg::QuaternionStamped& in)
337 {
338  return in;
339 }
340 
346 inline
347 void fromMsg(const geometry_msgs::msg::QuaternionStamped& msg, geometry_msgs::msg::QuaternionStamped& out)
348 {
349  out = msg;
350 }
351 
357 inline
358 geometry_msgs::msg::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
359 {
360  geometry_msgs::msg::QuaternionStamped out;
361  out.header.stamp = tf2_ros::toMsg(in.stamp_);
362  out.header.frame_id = in.frame_id_;
363  out.quaternion.w = in.getW();
364  out.quaternion.x = in.getX();
365  out.quaternion.y = in.getY();
366  out.quaternion.z = in.getZ();
367  return out;
368 }
369 
375 inline
376 void fromMsg(const geometry_msgs::msg::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
377 {
378  out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
379  out.frame_id_ = in.header.frame_id;
380  tf2::Quaternion tmp;
381  fromMsg(in.quaternion, tmp);
382  out.setData(tmp);
383 }
384 
385 
386 /***************/
388 /***************/
389 
395 inline
396 geometry_msgs::msg::Transform toMsg(const tf2::Transform& in)
397 {
398  geometry_msgs::msg::Transform out;
399  out.translation.x = in.getOrigin().getX();
400  out.translation.y = in.getOrigin().getY();
401  out.translation.z = in.getOrigin().getZ();
402  out.rotation = toMsg(in.getRotation());
403  return out;
404 }
405 
411 inline
412 void fromMsg(const geometry_msgs::msg::Transform& in, tf2::Transform& out)
413 {
414  out.setOrigin(tf2::Vector3(in.translation.x, in.translation.y, in.translation.z));
415  // w at the end in the constructor
416  out.setRotation(tf2::Quaternion(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w));
417 }
418 
419 
420 /**********************/
422 /**********************/
423 
429 template <>
430 inline
431 tf2::TimePoint getTimestamp(const geometry_msgs::msg::TransformStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
432 
438 template <>
439 inline
440 std::string getFrameId(const geometry_msgs::msg::TransformStamped& t) {return t.header.frame_id;}
441 
448 template <>
449 inline
450 void doTransform(const geometry_msgs::msg::TransformStamped& t_in, geometry_msgs::msg::TransformStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
451  {
452  KDL::Vector v(t_in.transform.translation.x, t_in.transform.translation.y,
453  t_in.transform.translation.z);
454  KDL::Rotation r = KDL::Rotation::Quaternion(t_in.transform.rotation.x,
455  t_in.transform.rotation.y, t_in.transform.rotation.z, t_in.transform.rotation.w);
456 
457  KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
458  t_out.transform.translation.x = v_out.p[0];
459  t_out.transform.translation.y = v_out.p[1];
460  t_out.transform.translation.z = v_out.p[2];
461  v_out.M.GetQuaternion(t_out.transform.rotation.x, t_out.transform.rotation.y,
462  t_out.transform.rotation.z, t_out.transform.rotation.w);
463  t_out.header.stamp = transform.header.stamp;
464  t_out.header.frame_id = transform.header.frame_id;
465  }
466 
472 inline
473 geometry_msgs::msg::TransformStamped toMsg(const geometry_msgs::msg::TransformStamped& in)
474 {
475  return in;
476 }
477 
483 inline
484 void fromMsg(const geometry_msgs::msg::TransformStamped& msg, geometry_msgs::msg::TransformStamped& out)
485 {
486  out = msg;
487 }
488 
494 inline
495 void fromMsg(const geometry_msgs::msg::TransformStamped& in, tf2::Stamped <tf2::Transform>& out)
496 {
497  out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
498  out.frame_id_ = in.header.frame_id;
499  tf2::Transform tmp;
500  fromMsg(in.transform, tmp);
501  out.setData(tmp);
502 }
503 
509 inline
510 geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped<tf2::Transform>& in)
511 {
512  geometry_msgs::msg::TransformStamped out;
513  out.header.stamp = tf2_ros::toMsg(in.stamp_);
514  out.header.frame_id = in.frame_id_;
515  out.transform.translation.x = in.getOrigin().getX();
516  out.transform.translation.y = in.getOrigin().getY();
517  out.transform.translation.z = in.getOrigin().getZ();
518  out.transform.rotation = toMsg(in.getRotation());
519  return out;
520 }
521 
522 /**********/
524 /**********/
525 
530 inline
532 void toMsg(const tf2::Transform& in, geometry_msgs::msg::Pose& out )
533 {
534  out.position.x = in.getOrigin().getX();
535  out.position.y = in.getOrigin().getY();
536  out.position.z = in.getOrigin().getZ();
537  out.orientation = toMsg(in.getRotation());
538 }
539 
544 inline
545 void fromMsg(const geometry_msgs::msg::Pose& in, tf2::Transform& out)
546 {
547  out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z));
548  // w at the end in the constructor
549  out.setRotation(tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w));
550 }
551 
552 } // namespace
553 
554 #endif // TF2_GEOMETRY_MSGS_H
std::string
tf2::Stamped::stamp_
TimePoint stamp_
tf2::Stamped::setData
void setData(const T &input)
tf2::fromMsg
void fromMsg(const A &, B &b)
tf2::Quaternion::getW
const TF2SIMD_FORCE_INLINE tf2Scalar & getW() const
tf2::Transform::setRotation
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
tf2::Transform::getOrigin
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
tf2::Stamped
tf2::getFrameId
std::string getFrameId(const T &t)
tf2::Stamped::frame_id_
std::string frame_id_
tf2::Transform::setOrigin
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
Quaternion.h
tf2::Transform
tf2::Transform::getRotation
Quaternion getRotation() const
std::chrono::time_point
tf2_ros::fromMsg
tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time &time_msg)
tf2
tf2::Quaternion
tf2::toMsg
B toMsg(const A &a)
convert.h
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::msg::TransformStamped &transform)
tf2::getTimestamp
tf2::TimePoint getTimestamp(const T &t)
tf2::gmTransformToKDL
KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped &t)
Convert a TransformStamped message to a KDL frame.
Definition: tf2_geometry_msgs.h:55
Transform.h
tf2_ros::toMsg
builtin_interfaces::msg::Time toMsg(const tf2::TimePoint &t)
buffer_interface.h
Generated by   doxygen 1.8.17