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 <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
46 #include <kdl/frames.hpp>
47 
48 namespace tf2
49 {
50 
55 inline
56 KDL::Frame gmTransformToKDL(const geometry_msgs::msg::TransformStamped& t)
57  {
58  return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y,
59  t.transform.rotation.z, t.transform.rotation.w),
60  KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
61  }
62 
63 
64 /********************/
66 /********************/
67 
73 template <>
74 inline
75  tf2::TimePoint getTimestamp(const geometry_msgs::msg::Vector3Stamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
76 
82 template <>
83 inline
84  std::string getFrameId(const geometry_msgs::msg::Vector3Stamped& t) {return t.header.frame_id;}
85 
92 template <>
93 inline
94  void doTransform(const geometry_msgs::msg::Vector3Stamped& t_in, geometry_msgs::msg::Vector3Stamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
95  {
96  KDL::Vector v_out = gmTransformToKDL(transform).M * KDL::Vector(t_in.vector.x, t_in.vector.y, t_in.vector.z);
97  t_out.vector.x = v_out[0];
98  t_out.vector.y = v_out[1];
99  t_out.vector.z = v_out[2];
100  t_out.header.stamp = transform.header.stamp;
101  t_out.header.frame_id = transform.header.frame_id;
102  }
103 
109 inline
110 geometry_msgs::msg::Vector3Stamped toMsg(const geometry_msgs::msg::Vector3Stamped& in)
111 {
112  return in;
113 }
114 
120 inline
121 void fromMsg(const geometry_msgs::msg::Vector3Stamped& msg, geometry_msgs::msg::Vector3Stamped& out)
122 {
123  out = msg;
124 }
125 
126 
127 
128 /******************/
130 /******************/
131 
137 template <>
138 inline
139  tf2::TimePoint getTimestamp(const geometry_msgs::msg::PointStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
140 
146 template <>
147 inline
148  std::string getFrameId(const geometry_msgs::msg::PointStamped& t) {return t.header.frame_id;}
149 
156 template <>
157 inline
158  void doTransform(const geometry_msgs::msg::PointStamped& t_in, geometry_msgs::msg::PointStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
159  {
160  KDL::Vector v_out = gmTransformToKDL(transform) * KDL::Vector(t_in.point.x, t_in.point.y, t_in.point.z);
161  t_out.point.x = v_out[0];
162  t_out.point.y = v_out[1];
163  t_out.point.z = v_out[2];
164  t_out.header.stamp = transform.header.stamp;
165  t_out.header.frame_id = transform.header.frame_id;
166  }
167 
173 inline
174 geometry_msgs::msg::PointStamped toMsg(const geometry_msgs::msg::PointStamped& in)
175 {
176  return in;
177 }
178 
184 inline
185 void fromMsg(const geometry_msgs::msg::PointStamped& msg, geometry_msgs::msg::PointStamped& out)
186 {
187  out = msg;
188 }
189 
190 
191 /*****************/
193 /*****************/
194 
200 template <>
201 inline
202  tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
203 
209 template <>
210 inline
211  std::string getFrameId(const geometry_msgs::msg::PoseStamped& t) {return t.header.frame_id;}
212 
219 template <>
220 inline
221  void doTransform(const geometry_msgs::msg::PoseStamped& t_in, geometry_msgs::msg::PoseStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
222  {
223  KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z);
224  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);
225 
226  KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
227  t_out.pose.position.x = v_out.p[0];
228  t_out.pose.position.y = v_out.p[1];
229  t_out.pose.position.z = v_out.p[2];
230  v_out.M.GetQuaternion(t_out.pose.orientation.x, t_out.pose.orientation.y, t_out.pose.orientation.z, t_out.pose.orientation.w);
231  t_out.header.stamp = transform.header.stamp;
232  t_out.header.frame_id = transform.header.frame_id;
233  }
234 
240 inline
241 geometry_msgs::msg::PoseStamped toMsg(const geometry_msgs::msg::PoseStamped& in)
242 {
243  return in;
244 }
245 
251 inline
252 void fromMsg(const geometry_msgs::msg::PoseStamped& msg, geometry_msgs::msg::PoseStamped& out)
253 {
254  out = msg;
255 }
256 
257 
258 /*******************************/
260 /*******************************/
261 
267 template <>
268 inline
269  tf2::TimePoint getTimestamp(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
270 
276 template <>
277 inline
278  std::string getFrameId(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return t.header.frame_id;}
279 
285 template <>
286 inline
287  std::array<std::array<double, 6>, 6> getCovarianceMatrix(const geometry_msgs::msg::PoseWithCovarianceStamped& t) {return covarianceRowMajorToNested(t.pose.covariance);}
288 
295 template <>
296 inline
297  void doTransform(const geometry_msgs::msg::PoseWithCovarianceStamped& t_in, geometry_msgs::msg::PoseWithCovarianceStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
298  {
299  KDL::Vector v(t_in.pose.pose.position.x, t_in.pose.pose.position.y, t_in.pose.pose.position.z);
300  KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.pose.orientation.x, t_in.pose.pose.orientation.y, t_in.pose.pose.orientation.z, t_in.pose.pose.orientation.w);
301 
302  KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
303  t_out.pose.pose.position.x = v_out.p[0];
304  t_out.pose.pose.position.y = v_out.p[1];
305  t_out.pose.pose.position.z = v_out.p[2];
306  v_out.M.GetQuaternion(t_out.pose.pose.orientation.x, t_out.pose.pose.orientation.y, t_out.pose.pose.orientation.z, t_out.pose.pose.orientation.w);
307  t_out.header.stamp = transform.header.stamp;
308  t_out.header.frame_id = transform.header.frame_id;
309  t_out.pose.covariance = t_in.pose.covariance;
310  }
311 
317 inline
318 geometry_msgs::msg::PoseWithCovarianceStamped toMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in)
319 {
320  return in;
321 }
322 
328 inline
329 void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& msg, geometry_msgs::msg::PoseWithCovarianceStamped& out)
330 {
331  out = msg;
332 }
333 
339 template<>
340 inline
341 geometry_msgs::msg::PoseWithCovarianceStamped toMsg(const tf2::WithCovarianceStamped<tf2::Transform>& in)
342 {
343  geometry_msgs::msg::PoseWithCovarianceStamped out;
344  out.header.stamp = tf2_ros::toMsg(in.stamp_);
345  out.header.frame_id = in.frame_id_;
346  out.pose.covariance = covarianceNestedToRowMajor(in.cov_mat_);
347  out.pose.pose.orientation.x = in.getRotation().getX();
348  out.pose.pose.orientation.y = in.getRotation().getY();
349  out.pose.pose.orientation.z = in.getRotation().getZ();
350  out.pose.pose.orientation.w = in.getRotation().getW();
351  out.pose.pose.position.x = in.getOrigin().getX();
352  out.pose.pose.position.y = in.getOrigin().getY();
353  out.pose.pose.position.z = in.getOrigin().getZ();
354  return out;
355 }
356 
362 template<>
363 inline
364 void fromMsg(const geometry_msgs::msg::PoseWithCovarianceStamped& in, tf2::WithCovarianceStamped<tf2::Transform>& out)
365 {
366  out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
367  out.frame_id_ = in.header.frame_id;
368  out.cov_mat_ = covarianceRowMajorToNested(in.pose.covariance);
369  tf2::Transform tmp;
370  fromMsg(in.pose.pose, tmp);
371  out.setData(tmp);
372 }
373 
374 /****************/
376 /****************/
377 
383 inline
384 geometry_msgs::msg::Quaternion toMsg(const tf2::Quaternion& in)
385 {
386  geometry_msgs::msg::Quaternion out;
387  out.w = in.getW();
388  out.x = in.getX();
389  out.y = in.getY();
390  out.z = in.getZ();
391  return out;
392 }
393 
399 inline
400 void fromMsg(const geometry_msgs::msg::Quaternion& in, tf2::Quaternion& out)
401 {
402  // w at the end in the constructor
403  out = tf2::Quaternion(in.x, in.y, in.z, in.w);
404 }
405 
406 
407 /***********************/
409 /***********************/
410 
416 template <>
417 inline
418 tf2::TimePoint getTimestamp(const geometry_msgs::msg::QuaternionStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
419 
425 template <>
426 inline
427 std::string getFrameId(const geometry_msgs::msg::QuaternionStamped& t) {return t.header.frame_id;}
428 
435 template <>
436 inline
437 void doTransform(const geometry_msgs::msg::QuaternionStamped& t_in, geometry_msgs::msg::QuaternionStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
438 {
439  tf2::Quaternion q_out = tf2::Quaternion(transform.transform.rotation.x, transform.transform.rotation.y,
440  transform.transform.rotation.z, transform.transform.rotation.w)*
441  tf2::Quaternion(t_in.quaternion.x, t_in.quaternion.y, t_in.quaternion.z, t_in.quaternion.w);
442  t_out.quaternion = toMsg(q_out);
443  t_out.header.stamp = transform.header.stamp;
444  t_out.header.frame_id = transform.header.frame_id;
445 }
446 
452 inline
453 geometry_msgs::msg::QuaternionStamped toMsg(const geometry_msgs::msg::QuaternionStamped& in)
454 {
455  return in;
456 }
457 
463 inline
464 void fromMsg(const geometry_msgs::msg::QuaternionStamped& msg, geometry_msgs::msg::QuaternionStamped& out)
465 {
466  out = msg;
467 }
468 
474 inline
475 geometry_msgs::msg::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
476 {
477  geometry_msgs::msg::QuaternionStamped out;
478  out.header.stamp = tf2_ros::toMsg(in.stamp_);
479  out.header.frame_id = in.frame_id_;
480  out.quaternion.w = in.getW();
481  out.quaternion.x = in.getX();
482  out.quaternion.y = in.getY();
483  out.quaternion.z = in.getZ();
484  return out;
485 }
486 
492 inline
493 void fromMsg(const geometry_msgs::msg::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
494 {
495  out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
496  out.frame_id_ = in.header.frame_id;
497  tf2::Quaternion tmp;
498  fromMsg(in.quaternion, tmp);
499  out.setData(tmp);
500 }
501 
502 
503 /***************/
505 /***************/
506 
512 inline
513 geometry_msgs::msg::Transform toMsg(const tf2::Transform& in)
514 {
515  geometry_msgs::msg::Transform out;
516  out.translation.x = in.getOrigin().getX();
517  out.translation.y = in.getOrigin().getY();
518  out.translation.z = in.getOrigin().getZ();
519  out.rotation = toMsg(in.getRotation());
520  return out;
521 }
522 
528 inline
529 void fromMsg(const geometry_msgs::msg::Transform& in, tf2::Transform& out)
530 {
531  out.setOrigin(tf2::Vector3(in.translation.x, in.translation.y, in.translation.z));
532  // w at the end in the constructor
533  out.setRotation(tf2::Quaternion(in.rotation.x, in.rotation.y, in.rotation.z, in.rotation.w));
534 }
535 
536 
537 /**********************/
539 /**********************/
540 
546 template <>
547 inline
548 tf2::TimePoint getTimestamp(const geometry_msgs::msg::TransformStamped& t) {return tf2_ros::fromMsg(t.header.stamp);}
549 
555 template <>
556 inline
557 std::string getFrameId(const geometry_msgs::msg::TransformStamped& t) {return t.header.frame_id;}
558 
565 template <>
566 inline
567 void doTransform(const geometry_msgs::msg::TransformStamped& t_in, geometry_msgs::msg::TransformStamped& t_out, const geometry_msgs::msg::TransformStamped& transform)
568  {
569  KDL::Vector v(t_in.transform.translation.x, t_in.transform.translation.y,
570  t_in.transform.translation.z);
571  KDL::Rotation r = KDL::Rotation::Quaternion(t_in.transform.rotation.x,
572  t_in.transform.rotation.y, t_in.transform.rotation.z, t_in.transform.rotation.w);
573 
574  KDL::Frame v_out = gmTransformToKDL(transform) * KDL::Frame(r, v);
575  t_out.transform.translation.x = v_out.p[0];
576  t_out.transform.translation.y = v_out.p[1];
577  t_out.transform.translation.z = v_out.p[2];
578  v_out.M.GetQuaternion(t_out.transform.rotation.x, t_out.transform.rotation.y,
579  t_out.transform.rotation.z, t_out.transform.rotation.w);
580  t_out.header.stamp = transform.header.stamp;
581  t_out.header.frame_id = transform.header.frame_id;
582  }
583 
589 inline
590 geometry_msgs::msg::TransformStamped toMsg(const geometry_msgs::msg::TransformStamped& in)
591 {
592  return in;
593 }
594 
600 inline
601 void fromMsg(const geometry_msgs::msg::TransformStamped& msg, geometry_msgs::msg::TransformStamped& out)
602 {
603  out = msg;
604 }
605 
611 inline
612 void fromMsg(const geometry_msgs::msg::TransformStamped& in, tf2::Stamped <tf2::Transform>& out)
613 {
614  out.stamp_ = tf2_ros::fromMsg(in.header.stamp);
615  out.frame_id_ = in.header.frame_id;
616  tf2::Transform tmp;
617  fromMsg(in.transform, tmp);
618  out.setData(tmp);
619 }
620 
626 inline
627 geometry_msgs::msg::TransformStamped toMsg(const tf2::Stamped<tf2::Transform>& in)
628 {
629  geometry_msgs::msg::TransformStamped out;
630  out.header.stamp = tf2_ros::toMsg(in.stamp_);
631  out.header.frame_id = in.frame_id_;
632  out.transform.translation.x = in.getOrigin().getX();
633  out.transform.translation.y = in.getOrigin().getY();
634  out.transform.translation.z = in.getOrigin().getZ();
635  out.transform.rotation = toMsg(in.getRotation());
636  return out;
637 }
638 
639 /**********/
641 /**********/
642 
647 inline
649 void toMsg(const tf2::Transform& in, geometry_msgs::msg::Pose& out )
650 {
651  out.position.x = in.getOrigin().getX();
652  out.position.y = in.getOrigin().getY();
653  out.position.z = in.getOrigin().getZ();
654  out.orientation = toMsg(in.getRotation());
655 }
656 
661 inline
662 void fromMsg(const geometry_msgs::msg::Pose& in, tf2::Transform& out)
663 {
664  out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z));
665  // w at the end in the constructor
666  out.setRotation(tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w));
667 }
668 
669 } // namespace
670 
671 #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::covarianceRowMajorToNested
std::array< std::array< double, 6 >, 6 > covarianceRowMajorToNested(const std::array< double, 36 > &row_major)
tf2::covarianceNestedToRowMajor
std::array< double, 36 > covarianceNestedToRowMajor(const std::array< std::array< double, 6 >, 6 > &nested_array)
tf2::Stamped::frame_id_
std::string frame_id_
tf2::Transform::setOrigin
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
Quaternion.h
std::array
tf2::Transform
tf2::Transform::getRotation
Quaternion getRotation() const
std::chrono::time_point
tf2::WithCovarianceStamped::cov_mat_
std::array< std::array< double, 6 >, 6 > cov_mat_
tf2::getCovarianceMatrix
std::array< std::array< double, 6 >, 6 > getCovarianceMatrix(const T &t)
tf2::WithCovarianceStamped::setData
void setData(const T &input)
tf2_ros::fromMsg
tf2::TimePoint fromMsg(const builtin_interfaces::msg::Time &time_msg)
tf2::WithCovarianceStamped
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:56
Transform.h
tf2_ros::toMsg
builtin_interfaces::msg::Time toMsg(const tf2::TimePoint &t)
buffer_interface.h
tf2::WithCovarianceStamped::frame_id_
std::string frame_id_
tf2::WithCovarianceStamped::stamp_
TimePoint stamp_
Generated by   doxygen 1.8.17