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 <geometry_msgs/PointStamped.h>
39 #include <geometry_msgs/QuaternionStamped.h>
40 #include <geometry_msgs/TransformStamped.h>
41 #include <geometry_msgs/Vector3Stamped.h>
42 #include <geometry_msgs/Pose.h>
43 #include <geometry_msgs/PoseStamped.h>
44 #include <geometry_msgs/PoseWithCovarianceStamped.h>
45 #include <geometry_msgs/Wrench.h>
46 #include <geometry_msgs/WrenchStamped.h>
47 #include <kdl/frames.hpp>
48 
49 #include <array>
50 
51 #include "ros/macros.h"
52 
53 namespace tf2
54 {
55 
61 inline
62 ROS_DEPRECATED KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t);
63 inline
64 KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t)
65  {
66  return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y,
67  t.transform.rotation.z, t.transform.rotation.w),
68  KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
69  }
70 
71 
72 /*************/
74 /*************/
75 
81 inline
82 geometry_msgs::Vector3 toMsg(const tf2::Vector3& in)
83 {
84  geometry_msgs::Vector3 out;
85  out.x = in.getX();
86  out.y = in.getY();
87  out.z = in.getZ();
88  return out;
89 }
90 
96 inline
97 void fromMsg(const geometry_msgs::Vector3& in, tf2::Vector3& out)
98 {
99  out = tf2::Vector3(in.x, in.y, in.z);
100 }
101 
102 
103 /********************/
105 /********************/
106 
113 template <>
114 inline
115  const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;}
116 
123 template <>
124 inline
125  const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;}
126 
127 
133 inline
134 geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in)
135 {
136  return in;
137 }
138 
144 inline
145 void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out)
146 {
147  out = msg;
148 }
149 
155 inline
156 geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped<tf2::Vector3>& in)
157 {
158  geometry_msgs::Vector3Stamped out;
159  out.header.stamp = in.stamp_;
160  out.header.frame_id = in.frame_id_;
161  out.vector.x = in.getX();
162  out.vector.y = in.getY();
163  out.vector.z = in.getZ();
164  return out;
165 }
166 
172 inline
173 void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped<tf2::Vector3>& out)
174 {
175  out.stamp_ = msg.header.stamp;
176  out.frame_id_ = msg.header.frame_id;
177  out.setData(tf2::Vector3(msg.vector.x, msg.vector.y, msg.vector.z));
178 }
179 
180 
181 /***********/
183 /***********/
184 
190 inline
191 geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out)
192 {
193  out.x = in.getX();
194  out.y = in.getY();
195  out.z = in.getZ();
196  return out;
197 }
198 
204 inline
205 void fromMsg(const geometry_msgs::Point& in, tf2::Vector3& out)
206 {
207  out = tf2::Vector3(in.x, in.y, in.z);
208 }
209 
210 
211 /******************/
213 /******************/
214 
221 template <>
222 inline
223  const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;}
224 
231 template <>
232 inline
233  const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;}
234 
240 inline
241 geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in)
242 {
243  return in;
244 }
245 
251 inline
252 void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out)
253 {
254  out = msg;
255 }
256 
262 inline
263 geometry_msgs::PointStamped toMsg(const tf2::Stamped<tf2::Vector3>& in, geometry_msgs::PointStamped & out)
264 {
265  out.header.stamp = in.stamp_;
266  out.header.frame_id = in.frame_id_;
267  out.point.x = in.getX();
268  out.point.y = in.getY();
269  out.point.z = in.getZ();
270  return out;
271 }
272 
278 inline
279 void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<tf2::Vector3>& out)
280 {
281  out.stamp_ = msg.header.stamp;
282  out.frame_id_ = msg.header.frame_id;
283  out.setData(tf2::Vector3(msg.point.x, msg.point.y, msg.point.z));
284 }
285 
286 
287 /****************/
289 /****************/
290 
296 inline
297 geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in)
298 {
299  geometry_msgs::Quaternion out;
300  out.w = in.getW();
301  out.x = in.getX();
302  out.y = in.getY();
303  out.z = in.getZ();
304  return out;
305 }
306 
312 inline
313 void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out)
314 {
315  // w at the end in the constructor
316  out = tf2::Quaternion(in.x, in.y, in.z, in.w);
317 }
318 
319 
320 /***********************/
322 /***********************/
323 
330 template <>
331 inline
332 const ros::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;}
333 
340 template <>
341 inline
342 const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;}
343 
349 inline
350 geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in)
351 {
352  return in;
353 }
354 
360 inline
361 void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out)
362 {
363  out = msg;
364 }
365 
371 inline
372 geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
373 {
374  geometry_msgs::QuaternionStamped out;
375  out.header.stamp = in.stamp_;
376  out.header.frame_id = in.frame_id_;
377  out.quaternion.w = in.getW();
378  out.quaternion.x = in.getX();
379  out.quaternion.y = in.getY();
380  out.quaternion.z = in.getZ();
381  return out;
382 }
383 
384 template <>
385 inline
386 ROS_DEPRECATED geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in);
387 
388 
389 //Backwards compatibility remove when forked for Lunar or newer
390 template <>
391 inline
392 geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
393 {
394  return toMsg(in);
395 }
396 
402 inline
403 void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
404 {
405  out.stamp_ = in.header.stamp;
406  out.frame_id_ = in.header.frame_id;
407  tf2::Quaternion tmp;
408  fromMsg(in.quaternion, tmp);
409  out.setData(tmp);
410 }
411 
412 template<>
413 inline
414 ROS_DEPRECATED void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out);
415 
416 //Backwards compatibility remove when forked for Lunar or newer
417 template<>
418 inline
419 void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
420 {
421  fromMsg(in, out);
422 }
423 
424 /**********/
426 /**********/
427 
432 inline
433 geometry_msgs::Pose& toMsg(const tf2::Transform& in, geometry_msgs::Pose& out)
434 {
435  toMsg(in.getOrigin(), out.position);
436  out.orientation = toMsg(in.getRotation());
437  return out;
438 }
439 
444 inline
445 void fromMsg(const geometry_msgs::Pose& in, tf2::Transform& out)
446 {
447  out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z));
448  // w at the end in the constructor
449  out.setRotation(tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w));
450 }
451 
452 
453 
454 /*****************/
456 /*****************/
457 
463 template <>
464 inline
465  const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;}
466 
472 template <>
473 inline
474  const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;}
475 
481 inline
482 geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in)
483 {
484  return in;
485 }
486 
492 inline
493 void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out)
494 {
495  out = msg;
496 }
497 
503 inline
504 geometry_msgs::PoseStamped toMsg(const tf2::Stamped<tf2::Transform>& in, geometry_msgs::PoseStamped & out)
505 {
506  out.header.stamp = in.stamp_;
507  out.header.frame_id = in.frame_id_;
508  toMsg(in.getOrigin(), out.pose.position);
509  out.pose.orientation = toMsg(in.getRotation());
510  return out;
511 }
512 
518 inline
519 void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<tf2::Transform>& out)
520 {
521  out.stamp_ = msg.header.stamp;
522  out.frame_id_ = msg.header.frame_id;
523  tf2::Transform tmp;
524  fromMsg(msg.pose, tmp);
525  out.setData(tmp);
526 }
527 
528 /*******************************/
530 /*******************************/
531 
537 template <>
538 inline
539  const ros::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.stamp;}
540 
546 template <>
547 inline
548  const std::string& getFrameId(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.frame_id;}
549 
555 inline
556 geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCovarianceStamped& in)
557 {
558  return in;
559 }
560 
566 inline
567 void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs::PoseWithCovarianceStamped& out)
568 {
569  out = msg;
570 }
571 
577 inline
578 geometry_msgs::PoseWithCovarianceStamped toMsg(const tf2::Stamped<tf2::Transform>& in, geometry_msgs::PoseWithCovarianceStamped & out)
579 {
580  out.header.stamp = in.stamp_;
581  out.header.frame_id = in.frame_id_;
582  toMsg(in.getOrigin(), out.pose.pose.position);
583  out.pose.pose.orientation = toMsg(in.getRotation());
584  return out;
585 }
586 
592 inline
593 void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf2::Stamped<tf2::Transform>& out)
594 {
595  out.stamp_ = msg.header.stamp;
596  out.frame_id_ = msg.header.frame_id;
597  tf2::Transform tmp;
598  fromMsg(msg.pose, tmp);
599  out.setData(tmp);
600 }
601 
602 /***************/
604 /***************/
605 
611 inline
612 geometry_msgs::Transform toMsg(const tf2::Transform& in)
613 {
614  geometry_msgs::Transform out;
615  out.translation = toMsg(in.getOrigin());
616  out.rotation = toMsg(in.getRotation());
617  return out;
618 }
619 
625 inline
626 void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out)
627 {
628  tf2::Vector3 v;
629  fromMsg(in.translation, v);
630  out.setOrigin(v);
631  // w at the end in the constructor
632  tf2::Quaternion q;
633  fromMsg(in.rotation, q);
634  out.setRotation(q);
635 }
636 
637 
638 /**********************/
640 /**********************/
641 
647 template <>
648 inline
649 const ros::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;}
650 
656 template <>
657 inline
658 const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;}
659 
665 inline
666 geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in)
667 {
668  return in;
669 }
670 
676 inline
677 void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out)
678 {
679  out = msg;
680 }
681 
687 inline
688 geometry_msgs::TransformStamped toMsg(const tf2::Stamped<tf2::Transform>& in)
689 {
690  geometry_msgs::TransformStamped out;
691  out.header.stamp = in.stamp_;
692  out.header.frame_id = in.frame_id_;
693  out.transform.translation = toMsg(in.getOrigin());
694  out.transform.rotation = toMsg(in.getRotation());
695  return out;
696 }
697 
698 
704 inline
705 void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped<tf2::Transform>& out)
706 {
707  out.stamp_ = msg.header.stamp;
708  out.frame_id_ = msg.header.frame_id;
709  tf2::Transform tmp;
710  fromMsg(msg.transform, tmp);
711  out.setData(tmp);
712 }
713 
720 template <>
721 inline
722  void doTransform(const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out, const geometry_msgs::TransformStamped& transform)
723  {
724  tf2::Transform t;
725  fromMsg(transform.transform, t);
726  tf2::Vector3 v_in;
727  fromMsg(t_in, v_in);
728  tf2::Vector3 v_out = t * v_in;
729  toMsg(v_out, t_out);
730  }
731 
738 template <>
739 inline
740  void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform)
741  {
742  doTransform(t_in.point, t_out.point, transform);
743  t_out.header.stamp = transform.header.stamp;
744  t_out.header.frame_id = transform.header.frame_id;
745  }
746 
753 template <>
754 inline
755 void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternion& t_out, const geometry_msgs::TransformStamped& transform)
756 {
757  tf2::Quaternion t, q_in;
758  fromMsg(transform.transform.rotation, t);
759  fromMsg(t_in, q_in);
760 
761  tf2::Quaternion q_out = t * q_in;
762  t_out = toMsg(q_out);
763 }
764 
771 template <>
772 inline
773 void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const geometry_msgs::TransformStamped& transform)
774 {
775  doTransform(t_in.quaternion, t_out.quaternion, transform);
776  t_out.header.stamp = transform.header.stamp;
777  t_out.header.frame_id = transform.header.frame_id;
778 }
779 
780 
787 template <>
788 inline
789 void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, const geometry_msgs::TransformStamped& transform)
790 {
791  tf2::Vector3 v;
792  fromMsg(t_in.position, v);
793  tf2::Quaternion r;
794  fromMsg(t_in.orientation, r);
795 
796  tf2::Transform t;
797  fromMsg(transform.transform, t);
798  tf2::Transform v_out = t * tf2::Transform(r, v);
799  toMsg(v_out, t_out);
800 }
801 
808 template <>
809 inline
810 void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform)
811 {
812  doTransform(t_in.pose, t_out.pose, transform);
813  t_out.header.stamp = transform.header.stamp;
814  t_out.header.frame_id = transform.header.frame_id;
815 }
816 
822 inline
823 geometry_msgs::PoseWithCovariance::_covariance_type transformCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type& cov_in, const tf2::Transform& transform)
824 {
837  // get rotation matrix transpose
838  const tf2::Matrix3x3 R_transpose = transform.getBasis().transpose();
839 
840  // convert the covariance matrix into four 3x3 blocks
841  const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2],
842  cov_in[6], cov_in[7], cov_in[8],
843  cov_in[12], cov_in[13], cov_in[14]);
844  const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5],
845  cov_in[9], cov_in[10], cov_in[11],
846  cov_in[15], cov_in[16], cov_in[17]);
847  const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20],
848  cov_in[24], cov_in[25], cov_in[26],
849  cov_in[30], cov_in[31], cov_in[32]);
850  const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23],
851  cov_in[27], cov_in[28], cov_in[29],
852  cov_in[33], cov_in[34], cov_in[35]);
853 
854  // perform blockwise matrix multiplication
855  const tf2::Matrix3x3 result_11 = transform.getBasis()*cov_11*R_transpose;
856  const tf2::Matrix3x3 result_12 = transform.getBasis()*cov_12*R_transpose;
857  const tf2::Matrix3x3 result_21 = transform.getBasis()*cov_21*R_transpose;
858  const tf2::Matrix3x3 result_22 = transform.getBasis()*cov_22*R_transpose;
859 
860  // form the output
861  geometry_msgs::PoseWithCovariance::_covariance_type output;
862  output[0] = result_11[0][0];
863  output[1] = result_11[0][1];
864  output[2] = result_11[0][2];
865  output[6] = result_11[1][0];
866  output[7] = result_11[1][1];
867  output[8] = result_11[1][2];
868  output[12] = result_11[2][0];
869  output[13] = result_11[2][1];
870  output[14] = result_11[2][2];
871 
872  output[3] = result_12[0][0];
873  output[4] = result_12[0][1];
874  output[5] = result_12[0][2];
875  output[9] = result_12[1][0];
876  output[10] = result_12[1][1];
877  output[11] = result_12[1][2];
878  output[15] = result_12[2][0];
879  output[16] = result_12[2][1];
880  output[17] = result_12[2][2];
881 
882  output[18] = result_21[0][0];
883  output[19] = result_21[0][1];
884  output[20] = result_21[0][2];
885  output[24] = result_21[1][0];
886  output[25] = result_21[1][1];
887  output[26] = result_21[1][2];
888  output[30] = result_21[2][0];
889  output[31] = result_21[2][1];
890  output[32] = result_21[2][2];
891 
892  output[21] = result_22[0][0];
893  output[22] = result_22[0][1];
894  output[23] = result_22[0][2];
895  output[27] = result_22[1][0];
896  output[28] = result_22[1][1];
897  output[29] = result_22[1][2];
898  output[33] = result_22[2][0];
899  output[34] = result_22[2][1];
900  output[35] = result_22[2][2];
901 
902  return output;
903 }
904 
911 template <>
912 inline
913 void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_msgs::PoseWithCovarianceStamped& t_out, const geometry_msgs::TransformStamped& transform)
914 {
915  tf2::Vector3 v;
916  fromMsg(t_in.pose.pose.position, v);
917  tf2::Quaternion r;
918  fromMsg(t_in.pose.pose.orientation, r);
919 
920  tf2::Transform t;
921  fromMsg(transform.transform, t);
922  tf2::Transform v_out = t * tf2::Transform(r, v);
923  toMsg(v_out, t_out.pose.pose);
924  t_out.header.stamp = transform.header.stamp;
925  t_out.header.frame_id = transform.header.frame_id;
926 
927  t_out.pose.covariance = transformCovariance(t_in.pose.covariance, t);
928 }
929 
936 template <>
937 inline
938 void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform)
939  {
940  tf2::Transform input;
941  fromMsg(t_in.transform, input);
942 
943  tf2::Transform t;
944  fromMsg(transform.transform, t);
945  tf2::Transform v_out = t * input;
946 
947  t_out.transform = toMsg(v_out);
948  t_out.header.stamp = transform.header.stamp;
949  t_out.header.frame_id = transform.header.frame_id;
950  }
951 
958 template <>
959 inline
960  void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const geometry_msgs::TransformStamped& transform)
961  {
962  tf2::Transform t;
963  fromMsg(transform.transform, t);
964  tf2::Vector3 v_out = t.getBasis() * tf2::Vector3(t_in.x, t_in.y, t_in.z);
965  t_out.x = v_out[0];
966  t_out.y = v_out[1];
967  t_out.z = v_out[2];
968  }
969 
976 template <>
977 inline
978  void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform)
979  {
980  doTransform(t_in.vector, t_out.vector, transform);
981  t_out.header.stamp = transform.header.stamp;
982  t_out.header.frame_id = transform.header.frame_id;
983  }
984 
985 
986 /**********************/
987 /*** WrenchStamped ****/
988 /**********************/
989 template <>
990 inline
991 const ros::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return t.header.stamp;}
992 
993 
994 template <>
995 inline
996 const std::string& getFrameId(const geometry_msgs::WrenchStamped& t) {return t.header.frame_id;}
997 
998 
999 inline
1000 geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in)
1001 {
1002  return in;
1003 }
1004 
1005 inline
1006 void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out)
1007 {
1008  out = msg;
1009 }
1010 
1011 
1012 inline
1013 geometry_msgs::WrenchStamped toMsg(const tf2::Stamped<std::array<tf2::Vector3, 2>>& in, geometry_msgs::WrenchStamped & out)
1014 {
1015  out.header.stamp = in.stamp_;
1016  out.header.frame_id = in.frame_id_;
1017  out.wrench.force = toMsg(in[0]);
1018  out.wrench.torque = toMsg(in[1]);
1019  return out;
1020 }
1021 
1022 
1023 inline
1024 void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped<std::array<tf2::Vector3, 2>>& out)
1025 {
1026  out.stamp_ = msg.header.stamp;
1027  out.frame_id_ = msg.header.frame_id;
1028  tf2::Vector3 tmp;
1029  fromMsg(msg.wrench.force, tmp);
1030  tf2::Vector3 tmp1;
1031  fromMsg(msg.wrench.torque, tmp1);
1032  std::array<tf2::Vector3, 2> tmp_array;
1033  tmp_array[0] = tmp;
1034  tmp_array[1] = tmp1;
1035  out.setData(tmp_array);
1036 }
1037 
1038 template<>
1039 inline
1040 void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out, const geometry_msgs::TransformStamped& transform)
1041 {
1042  doTransform(t_in.force, t_out.force, transform);
1043  doTransform(t_in.torque, t_out.torque, transform);
1044 }
1045 
1046 
1047 template<>
1048 inline
1049 void doTransform(const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out, const geometry_msgs::TransformStamped& transform)
1050 {
1051  doTransform(t_in.wrench, t_out.wrench, transform);
1052  t_out.header.stamp = transform.header.stamp;
1053  t_out.header.frame_id = transform.header.frame_id;
1054 }
1055 
1056 } // namespace
1057 
1058 #endif // TF2_GEOMETRY_MSGS_H
tf2::getFrameId
const std::string & getFrameId(const T &t)
tf2::Transform::getBasis
TF2SIMD_FORCE_INLINE Matrix3x3 & getBasis()
tf2::Matrix3x3::transpose
Matrix3x3 transpose() const
tf2::Transform::setRotation
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
KDL::Rotation::Quaternion
static Rotation Quaternion(double x, double y, double z, double w)
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
t
geometry_msgs::TransformStamped t
Definition: test_tf2_geometry_msgs.cpp:41
tf2::Transform::getOrigin
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
tf2::transformCovariance
geometry_msgs::PoseWithCovariance::_covariance_type transformCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type &cov_in, const tf2::Transform &transform)
Transform the covariance matrix of a PoseWithCovarianceStamped message to a new frame.
Definition: tf2_geometry_msgs.h:823
tf2::gmTransformToKDL
ROS_DEPRECATED KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped &t)
Convert a TransformStamped message to a KDL frame.
Definition: tf2_geometry_msgs.h:64
tf2::Stamped::stamp_
ros::Time stamp_
tf2::fromMsg
void fromMsg(const A &, B &b)
tf2::Quaternion::getW
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
tf2::Transform::setOrigin
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
ROS_DEPRECATED
#define ROS_DEPRECATED
tf2::toMsg
B toMsg(const A &a)
tf2::Stamped::frame_id_
std::string frame_id_
tf2::Transform::getRotation
Quaternion getRotation() const
tf2::getTimestamp
const ros::Time & getTimestamp(const T &t)
tf2::Stamped::setData
void setData(const T &input)

tf2_geometry_msgs
Author(s): Wim Meeussen
autogenerated on Mon Jun 27 2022 02:43:20