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 <boost/array.hpp>
36 
37 #include <tf2/convert.h>
40 #include <geometry_msgs/PointStamped.h>
41 #include <geometry_msgs/QuaternionStamped.h>
42 #include <geometry_msgs/TransformStamped.h>
43 #include <geometry_msgs/Vector3Stamped.h>
44 #include <geometry_msgs/Pose.h>
45 #include <geometry_msgs/PoseStamped.h>
46 #include <geometry_msgs/PoseWithCovarianceStamped.h>
47 #include <geometry_msgs/Wrench.h>
48 #include <geometry_msgs/WrenchStamped.h>
49 #include <kdl/frames.hpp>
50 
51 namespace tf2
52 {
53 
59 inline
60 KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) __attribute__ ((deprecated));
61 inline
62 KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t)
63  {
64  return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y,
65  t.transform.rotation.z, t.transform.rotation.w),
66  KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
67  }
68 
69 
70 /*************/
72 /*************/
73 
79 inline
80 geometry_msgs::Vector3 toMsg(const tf2::Vector3& in)
81 {
82  geometry_msgs::Vector3 out;
83  out.x = in.getX();
84  out.y = in.getY();
85  out.z = in.getZ();
86  return out;
87 }
88 
94 inline
95 void fromMsg(const geometry_msgs::Vector3& in, tf2::Vector3& out)
96 {
97  out = tf2::Vector3(in.x, in.y, in.z);
98 }
99 
100 
101 /********************/
103 /********************/
104 
111 template <>
112 inline
113  const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;}
114 
121 template <>
122 inline
123  const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;}
124 
125 
131 inline
132 geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in)
133 {
134  return in;
135 }
136 
142 inline
143 void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out)
144 {
145  out = msg;
146 }
147 
153 inline
154 geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped<tf2::Vector3>& in)
155 {
156  geometry_msgs::Vector3Stamped out;
157  out.header.stamp = in.stamp_;
158  out.header.frame_id = in.frame_id_;
159  out.vector.x = in.getX();
160  out.vector.y = in.getY();
161  out.vector.z = in.getZ();
162  return out;
163 }
164 
170 inline
171 void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped<tf2::Vector3>& out)
172 {
173  out.stamp_ = msg.header.stamp;
174  out.frame_id_ = msg.header.frame_id;
175  out.setData(tf2::Vector3(msg.vector.x, msg.vector.y, msg.vector.z));
176 }
177 
178 
179 /***********/
181 /***********/
182 
188 inline
189 geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out)
190 {
191  out.x = in.getX();
192  out.y = in.getY();
193  out.z = in.getZ();
194  return out;
195 }
196 
202 inline
203 void fromMsg(const geometry_msgs::Point& in, tf2::Vector3& out)
204 {
205  out = tf2::Vector3(in.x, in.y, in.z);
206 }
207 
208 
209 /******************/
211 /******************/
212 
219 template <>
220 inline
221  const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;}
222 
229 template <>
230 inline
231  const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;}
232 
238 inline
239 geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in)
240 {
241  return in;
242 }
243 
249 inline
250 void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out)
251 {
252  out = msg;
253 }
254 
260 inline
261 geometry_msgs::PointStamped toMsg(const tf2::Stamped<tf2::Vector3>& in, geometry_msgs::PointStamped & out)
262 {
263  out.header.stamp = in.stamp_;
264  out.header.frame_id = in.frame_id_;
265  out.point.x = in.getX();
266  out.point.y = in.getY();
267  out.point.z = in.getZ();
268  return out;
269 }
270 
276 inline
277 void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<tf2::Vector3>& out)
278 {
279  out.stamp_ = msg.header.stamp;
280  out.frame_id_ = msg.header.frame_id;
281  out.setData(tf2::Vector3(msg.point.x, msg.point.y, msg.point.z));
282 }
283 
284 
285 /****************/
287 /****************/
288 
294 inline
295 geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in)
296 {
297  geometry_msgs::Quaternion out;
298  out.w = in.getW();
299  out.x = in.getX();
300  out.y = in.getY();
301  out.z = in.getZ();
302  return out;
303 }
304 
310 inline
311 void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out)
312 {
313  // w at the end in the constructor
314  out = tf2::Quaternion(in.x, in.y, in.z, in.w);
315 }
316 
317 
318 /***********************/
320 /***********************/
321 
328 template <>
329 inline
330 const ros::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;}
331 
338 template <>
339 inline
340 const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;}
341 
347 inline
348 geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in)
349 {
350  return in;
351 }
352 
358 inline
359 void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out)
360 {
361  out = msg;
362 }
363 
369 inline
370 geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
371 {
372  geometry_msgs::QuaternionStamped out;
373  out.header.stamp = in.stamp_;
374  out.header.frame_id = in.frame_id_;
375  out.quaternion.w = in.getW();
376  out.quaternion.x = in.getX();
377  out.quaternion.y = in.getY();
378  out.quaternion.z = in.getZ();
379  return out;
380 }
381 
382 template <>
383 inline
384 geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in) __attribute__ ((deprecated));
385 
386 
387 //Backwards compatibility remove when forked for Lunar or newer
388 template <>
389 inline
390 geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
391 {
392  return toMsg(in);
393 }
394 
400 inline
401 void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
402 {
403  out.stamp_ = in.header.stamp;
404  out.frame_id_ = in.header.frame_id;
405  tf2::Quaternion tmp;
406  fromMsg(in.quaternion, tmp);
407  out.setData(tmp);
408 }
409 
410 template<>
411 inline
412 void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out) __attribute__ ((deprecated));
413 
414 //Backwards compatibility remove when forked for Lunar or newer
415 template<>
416 inline
417 void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
418 {
419  fromMsg(in, out);
420 }
421 
422 /**********/
424 /**********/
425 
430 inline
431 geometry_msgs::Pose& toMsg(const tf2::Transform& in, geometry_msgs::Pose& out)
432 {
433  toMsg(in.getOrigin(), out.position);
434  out.orientation = toMsg(in.getRotation());
435  return out;
436 }
437 
442 inline
443 void fromMsg(const geometry_msgs::Pose& in, tf2::Transform& out)
444 {
445  out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z));
446  // w at the end in the constructor
447  out.setRotation(tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w));
448 }
449 
450 
451 
452 /*****************/
454 /*****************/
455 
461 template <>
462 inline
463  const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;}
464 
470 template <>
471 inline
472  const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;}
473 
479 inline
480 geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in)
481 {
482  return in;
483 }
484 
490 inline
491 void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out)
492 {
493  out = msg;
494 }
495 
501 inline
502 geometry_msgs::PoseStamped toMsg(const tf2::Stamped<tf2::Transform>& in, geometry_msgs::PoseStamped & out)
503 {
504  out.header.stamp = in.stamp_;
505  out.header.frame_id = in.frame_id_;
506  toMsg(in.getOrigin(), out.pose.position);
507  out.pose.orientation = toMsg(in.getRotation());
508  return out;
509 }
510 
516 inline
517 void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<tf2::Transform>& out)
518 {
519  out.stamp_ = msg.header.stamp;
520  out.frame_id_ = msg.header.frame_id;
521  tf2::Transform tmp;
522  fromMsg(msg.pose, tmp);
523  out.setData(tmp);
524 }
525 
526 /*******************************/
528 /*******************************/
529 
535 template <>
536 inline
537  const ros::Time& getTimestamp(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.stamp;}
538 
544 template <>
545 inline
546  const std::string& getFrameId(const geometry_msgs::PoseWithCovarianceStamped& t) {return t.header.frame_id;}
547 
553 inline
554 geometry_msgs::PoseWithCovarianceStamped toMsg(const geometry_msgs::PoseWithCovarianceStamped& in)
555 {
556  return in;
557 }
558 
564 inline
565 void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, geometry_msgs::PoseWithCovarianceStamped& out)
566 {
567  out = msg;
568 }
569 
575 inline
576 geometry_msgs::PoseWithCovarianceStamped toMsg(const tf2::Stamped<tf2::Transform>& in, geometry_msgs::PoseWithCovarianceStamped & out)
577 {
578  out.header.stamp = in.stamp_;
579  out.header.frame_id = in.frame_id_;
580  toMsg(in.getOrigin(), out.pose.pose.position);
581  out.pose.pose.orientation = toMsg(in.getRotation());
582  return out;
583 }
584 
590 inline
591 void fromMsg(const geometry_msgs::PoseWithCovarianceStamped& msg, tf2::Stamped<tf2::Transform>& out)
592 {
593  out.stamp_ = msg.header.stamp;
594  out.frame_id_ = msg.header.frame_id;
595  tf2::Transform tmp;
596  fromMsg(msg.pose, tmp);
597  out.setData(tmp);
598 }
599 
600 /***************/
602 /***************/
603 
609 inline
610 geometry_msgs::Transform toMsg(const tf2::Transform& in)
611 {
612  geometry_msgs::Transform out;
613  out.translation = toMsg(in.getOrigin());
614  out.rotation = toMsg(in.getRotation());
615  return out;
616 }
617 
623 inline
624 void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out)
625 {
626  tf2::Vector3 v;
627  fromMsg(in.translation, v);
628  out.setOrigin(v);
629  // w at the end in the constructor
630  tf2::Quaternion q;
631  fromMsg(in.rotation, q);
632  out.setRotation(q);
633 }
634 
635 
636 /**********************/
638 /**********************/
639 
645 template <>
646 inline
647 const ros::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;}
648 
654 template <>
655 inline
656 const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;}
657 
663 inline
664 geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in)
665 {
666  return in;
667 }
668 
674 inline
675 void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out)
676 {
677  out = msg;
678 }
679 
685 inline
686 geometry_msgs::TransformStamped toMsg(const tf2::Stamped<tf2::Transform>& in)
687 {
688  geometry_msgs::TransformStamped out;
689  out.header.stamp = in.stamp_;
690  out.header.frame_id = in.frame_id_;
691  out.transform.translation = toMsg(in.getOrigin());
692  out.transform.rotation = toMsg(in.getRotation());
693  return out;
694 }
695 
696 
702 inline
703 void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped<tf2::Transform>& out)
704 {
705  out.stamp_ = msg.header.stamp;
706  out.frame_id_ = msg.header.frame_id;
707  tf2::Transform tmp;
708  fromMsg(msg.transform, tmp);
709  out.setData(tmp);
710 }
711 
718 template <>
719 inline
720  void doTransform(const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out, const geometry_msgs::TransformStamped& transform)
721  {
723  fromMsg(transform.transform, t);
724  tf2::Vector3 v_in;
725  fromMsg(t_in, v_in);
726  tf2::Vector3 v_out = t * v_in;
727  toMsg(v_out, t_out);
728  }
729 
736 template <>
737 inline
738  void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform)
739  {
740  doTransform(t_in.point, t_out.point, transform);
741  t_out.header.stamp = transform.header.stamp;
742  t_out.header.frame_id = transform.header.frame_id;
743  }
744 
751 template <>
752 inline
753 void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternion& t_out, const geometry_msgs::TransformStamped& transform)
754 {
755  tf2::Quaternion t, q_in;
756  fromMsg(transform.transform.rotation, t);
757  fromMsg(t_in, q_in);
758 
759  tf2::Quaternion q_out = t * q_in;
760  t_out = toMsg(q_out);
761 }
762 
769 template <>
770 inline
771 void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const geometry_msgs::TransformStamped& transform)
772 {
773  doTransform(t_in.quaternion, t_out.quaternion, transform);
774  t_out.header.stamp = transform.header.stamp;
775  t_out.header.frame_id = transform.header.frame_id;
776 }
777 
778 
785 template <>
786 inline
787 void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, const geometry_msgs::TransformStamped& transform)
788 {
789  tf2::Vector3 v;
790  fromMsg(t_in.position, v);
791  tf2::Quaternion r;
792  fromMsg(t_in.orientation, r);
793 
795  fromMsg(transform.transform, t);
796  tf2::Transform v_out = t * tf2::Transform(r, v);
797  toMsg(v_out, t_out);
798 }
799 
806 template <>
807 inline
808 void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform)
809 {
810  doTransform(t_in.pose, t_out.pose, transform);
811  t_out.header.stamp = transform.header.stamp;
812  t_out.header.frame_id = transform.header.frame_id;
813 }
814 
820 inline
821 geometry_msgs::PoseWithCovariance::_covariance_type transformCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type& cov_in, const tf2::Transform& transform)
822 {
835  // get rotation matrix transpose
836  const tf2::Matrix3x3 R_transpose = transform.getBasis().transpose();
837 
838  // convert the covariance matrix into four 3x3 blocks
839  const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2],
840  cov_in[6], cov_in[7], cov_in[8],
841  cov_in[12], cov_in[13], cov_in[14]);
842  const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5],
843  cov_in[9], cov_in[10], cov_in[11],
844  cov_in[15], cov_in[16], cov_in[17]);
845  const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20],
846  cov_in[24], cov_in[25], cov_in[26],
847  cov_in[30], cov_in[31], cov_in[32]);
848  const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23],
849  cov_in[27], cov_in[28], cov_in[29],
850  cov_in[33], cov_in[34], cov_in[35]);
851 
852  // perform blockwise matrix multiplication
853  const tf2::Matrix3x3 result_11 = transform.getBasis()*cov_11*R_transpose;
854  const tf2::Matrix3x3 result_12 = transform.getBasis()*cov_12*R_transpose;
855  const tf2::Matrix3x3 result_21 = transform.getBasis()*cov_21*R_transpose;
856  const tf2::Matrix3x3 result_22 = transform.getBasis()*cov_22*R_transpose;
857 
858  // form the output
859  geometry_msgs::PoseWithCovariance::_covariance_type output;
860  output[0] = result_11[0][0];
861  output[1] = result_11[0][1];
862  output[2] = result_11[0][2];
863  output[6] = result_11[1][0];
864  output[7] = result_11[1][1];
865  output[8] = result_11[1][2];
866  output[12] = result_11[2][0];
867  output[13] = result_11[2][1];
868  output[14] = result_11[2][2];
869 
870  output[3] = result_12[0][0];
871  output[4] = result_12[0][1];
872  output[5] = result_12[0][2];
873  output[9] = result_12[1][0];
874  output[10] = result_12[1][1];
875  output[11] = result_12[1][2];
876  output[15] = result_12[2][0];
877  output[16] = result_12[2][1];
878  output[17] = result_12[2][2];
879 
880  output[18] = result_21[0][0];
881  output[19] = result_21[0][1];
882  output[20] = result_21[0][2];
883  output[24] = result_21[1][0];
884  output[25] = result_21[1][1];
885  output[26] = result_21[1][2];
886  output[30] = result_21[2][0];
887  output[31] = result_21[2][1];
888  output[32] = result_21[2][2];
889 
890  output[21] = result_22[0][0];
891  output[22] = result_22[0][1];
892  output[23] = result_22[0][2];
893  output[27] = result_22[1][0];
894  output[28] = result_22[1][1];
895  output[29] = result_22[1][2];
896  output[33] = result_22[2][0];
897  output[34] = result_22[2][1];
898  output[35] = result_22[2][2];
899 
900  return output;
901 }
902 
909 template <>
910 inline
911 void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_msgs::PoseWithCovarianceStamped& t_out, const geometry_msgs::TransformStamped& transform)
912 {
913  tf2::Vector3 v;
914  fromMsg(t_in.pose.pose.position, v);
915  tf2::Quaternion r;
916  fromMsg(t_in.pose.pose.orientation, r);
917 
919  fromMsg(transform.transform, t);
920  tf2::Transform v_out = t * tf2::Transform(r, v);
921  toMsg(v_out, t_out.pose.pose);
922  t_out.header.stamp = transform.header.stamp;
923  t_out.header.frame_id = transform.header.frame_id;
924 
925  t_out.pose.covariance = transformCovariance(t_in.pose.covariance, t);
926 }
927 
934 template <>
935 inline
936 void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform)
937  {
938  tf2::Transform input;
939  fromMsg(t_in.transform, input);
940 
942  fromMsg(transform.transform, t);
943  tf2::Transform v_out = t * input;
944 
945  t_out.transform = toMsg(v_out);
946  t_out.header.stamp = transform.header.stamp;
947  t_out.header.frame_id = transform.header.frame_id;
948  }
949 
956 template <>
957 inline
958  void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const geometry_msgs::TransformStamped& transform)
959  {
961  fromMsg(transform.transform, t);
962  tf2::Vector3 v_out = t.getBasis() * tf2::Vector3(t_in.x, t_in.y, t_in.z);
963  t_out.x = v_out[0];
964  t_out.y = v_out[1];
965  t_out.z = v_out[2];
966  }
967 
974 template <>
975 inline
976  void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform)
977  {
978  doTransform(t_in.vector, t_out.vector, transform);
979  t_out.header.stamp = transform.header.stamp;
980  t_out.header.frame_id = transform.header.frame_id;
981  }
982 
983 
984 /**********************/
985 /*** WrenchStamped ****/
986 /**********************/
987 template <>
988 inline
989 const ros::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return t.header.stamp;}
990 
991 
992 template <>
993 inline
994 const std::string& getFrameId(const geometry_msgs::WrenchStamped& t) {return t.header.frame_id;}
995 
996 
997 inline
998 geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in)
999 {
1000  return in;
1001 }
1002 
1003 inline
1004 void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out)
1005 {
1006  out = msg;
1007 }
1008 
1009 
1010 inline
1011 geometry_msgs::WrenchStamped toMsg(const tf2::Stamped< boost::array<tf2::Vector3, 2> >& in, geometry_msgs::WrenchStamped& out)
1012 {
1013  out.header.stamp = in.stamp_;
1014  out.header.frame_id = in.frame_id_;
1015  out.wrench.force = toMsg(in[0]);
1016  out.wrench.torque = toMsg(in[1]);
1017  return out;
1018 }
1019 
1020 
1021 inline
1022 void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped< boost::array<tf2::Vector3, 2> >& out)
1023 {
1024  out.stamp_ = msg.header.stamp;
1025  out.frame_id_ = msg.header.frame_id;
1026  tf2::Vector3 tmp;
1027  fromMsg(msg.wrench.force, tmp);
1028  tf2::Vector3 tmp1;
1029  fromMsg(msg.wrench.torque, tmp1);
1030  boost::array<tf2::Vector3, 2> tmp_array;
1031  tmp_array[0] = tmp;
1032  tmp_array[1] = tmp1;
1033  out.setData(tmp_array);
1034 }
1035 
1036 template<>
1037 inline
1038 void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out, const geometry_msgs::TransformStamped& transform)
1039 {
1040  doTransform(t_in.force, t_out.force, transform);
1041  doTransform(t_in.torque, t_out.torque, transform);
1042 }
1043 
1044 
1045 template<>
1046 inline
1047 void doTransform(const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out, const geometry_msgs::TransformStamped& transform)
1048 {
1049  doTransform(t_in.wrench, t_out.wrench, transform);
1050  t_out.header.stamp = transform.header.stamp;
1051  t_out.header.frame_id = transform.header.frame_id;
1052 }
1053 
1054 } // namespace
1055 
1056 #endif // TF2_GEOMETRY_MSGS_H
const std::string & getFrameId(const T &t)
TF2SIMD_FORCE_INLINE Matrix3x3 & getBasis()
Quaternion getRotation() const
TF2SIMD_FORCE_INLINE void setRotation(const Quaternion &q)
TF2SIMD_FORCE_INLINE const tf2Scalar & getW() const
KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped &t) __attribute__((deprecated))
Convert a TransformStamped message to a KDL frame.
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)
geometry_msgs::TransformStamped t
Matrix3x3 transpose() const
TF2SIMD_FORCE_INLINE Vector3 & getOrigin()
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.
ros::Time stamp_
void fromMsg(const A &, B &b)
TF2SIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
B toMsg(const A &a)
std::string frame_id_
const ros::Time & getTimestamp(const T &t)
void setData(const T &input)


tf2_geometry_msgs
Author(s): Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:59