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 }
675 inline
676 void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out)
677 {
678  out = msg;
679 }
680 
686 inline
687 geometry_msgs::TransformStamped toMsg(const tf2::Stamped<tf2::Transform>& in)
688 {
689  geometry_msgs::TransformStamped out;
690  out.header.stamp = in.stamp_;
691  out.header.frame_id = in.frame_id_;
692  out.transform.translation = toMsg(in.getOrigin());
693  out.transform.rotation = toMsg(in.getRotation());
694  return out;
695 }
696 
697 
703 inline
704 void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped<tf2::Transform>& out)
705 {
706  out.stamp_ = msg.header.stamp;
707  out.frame_id_ = msg.header.frame_id;
708  tf2::Transform tmp;
709  fromMsg(msg.transform, tmp);
710  out.setData(tmp);
711 }
712 
719 template <>
720 inline
721  void doTransform(const geometry_msgs::Point& t_in, geometry_msgs::Point& t_out, const geometry_msgs::TransformStamped& transform)
722  {
723  tf2::Transform t;
724  fromMsg(transform.transform, t);
725  tf2::Vector3 v_in;
726  fromMsg(t_in, v_in);
727  tf2::Vector3 v_out = t * v_in;
728  toMsg(v_out, t_out);
729  }
730 
737 template <>
738 inline
739  void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform)
740  {
741  doTransform(t_in.point, t_out.point, transform);
742  t_out.header.stamp = transform.header.stamp;
743  t_out.header.frame_id = transform.header.frame_id;
744  }
745 
752 template <>
753 inline
754 void doTransform(const geometry_msgs::Quaternion& t_in, geometry_msgs::Quaternion& t_out, const geometry_msgs::TransformStamped& transform)
755 {
756  tf2::Quaternion t, q_in;
757  fromMsg(transform.transform.rotation, t);
758  fromMsg(t_in, q_in);
759 
760  tf2::Quaternion q_out = t * q_in;
761  t_out = toMsg(q_out);
762 }
763 
770 template <>
771 inline
772 void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const geometry_msgs::TransformStamped& transform)
773 {
774  doTransform(t_in.quaternion, t_out.quaternion, transform);
775  t_out.header.stamp = transform.header.stamp;
776  t_out.header.frame_id = transform.header.frame_id;
777 }
778 
779 
786 template <>
787 inline
788 void doTransform(const geometry_msgs::Pose& t_in, geometry_msgs::Pose& t_out, const geometry_msgs::TransformStamped& transform)
789 {
790  tf2::Vector3 v;
791  fromMsg(t_in.position, v);
792  tf2::Quaternion r;
793  fromMsg(t_in.orientation, r);
794 
795  tf2::Transform t;
796  fromMsg(transform.transform, t);
797  tf2::Transform v_out = t * tf2::Transform(r, v);
798  toMsg(v_out, t_out);
799 }
800 
807 template <>
808 inline
809 void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform)
810 {
811  doTransform(t_in.pose, t_out.pose, transform);
812  t_out.header.stamp = transform.header.stamp;
813  t_out.header.frame_id = transform.header.frame_id;
814 }
815 
821 inline
822 geometry_msgs::PoseWithCovariance::_covariance_type transformCovariance(const geometry_msgs::PoseWithCovariance::_covariance_type& cov_in, const tf2::Transform& transform)
823 {
836  // get rotation matrix transpose
837  const tf2::Matrix3x3 R_transpose = transform.getBasis().transpose();
838 
839  // convert the covariance matrix into four 3x3 blocks
840  const tf2::Matrix3x3 cov_11(cov_in[0], cov_in[1], cov_in[2],
841  cov_in[6], cov_in[7], cov_in[8],
842  cov_in[12], cov_in[13], cov_in[14]);
843  const tf2::Matrix3x3 cov_12(cov_in[3], cov_in[4], cov_in[5],
844  cov_in[9], cov_in[10], cov_in[11],
845  cov_in[15], cov_in[16], cov_in[17]);
846  const tf2::Matrix3x3 cov_21(cov_in[18], cov_in[19], cov_in[20],
847  cov_in[24], cov_in[25], cov_in[26],
848  cov_in[30], cov_in[31], cov_in[32]);
849  const tf2::Matrix3x3 cov_22(cov_in[21], cov_in[22], cov_in[23],
850  cov_in[27], cov_in[28], cov_in[29],
851  cov_in[33], cov_in[34], cov_in[35]);
852 
853  // perform blockwise matrix multiplication
854  const tf2::Matrix3x3 result_11 = transform.getBasis()*cov_11*R_transpose;
855  const tf2::Matrix3x3 result_12 = transform.getBasis()*cov_12*R_transpose;
856  const tf2::Matrix3x3 result_21 = transform.getBasis()*cov_21*R_transpose;
857  const tf2::Matrix3x3 result_22 = transform.getBasis()*cov_22*R_transpose;
858 
859  // form the output
860  geometry_msgs::PoseWithCovariance::_covariance_type output;
861  output[0] = result_11[0][0];
862  output[1] = result_11[0][1];
863  output[2] = result_11[0][2];
864  output[6] = result_11[1][0];
865  output[7] = result_11[1][1];
866  output[8] = result_11[1][2];
867  output[12] = result_11[2][0];
868  output[13] = result_11[2][1];
869  output[14] = result_11[2][2];
870 
871  output[3] = result_12[0][0];
872  output[4] = result_12[0][1];
873  output[5] = result_12[0][2];
874  output[9] = result_12[1][0];
875  output[10] = result_12[1][1];
876  output[11] = result_12[1][2];
877  output[15] = result_12[2][0];
878  output[16] = result_12[2][1];
879  output[17] = result_12[2][2];
880 
881  output[18] = result_21[0][0];
882  output[19] = result_21[0][1];
883  output[20] = result_21[0][2];
884  output[24] = result_21[1][0];
885  output[25] = result_21[1][1];
886  output[26] = result_21[1][2];
887  output[30] = result_21[2][0];
888  output[31] = result_21[2][1];
889  output[32] = result_21[2][2];
890 
891  output[21] = result_22[0][0];
892  output[22] = result_22[0][1];
893  output[23] = result_22[0][2];
894  output[27] = result_22[1][0];
895  output[28] = result_22[1][1];
896  output[29] = result_22[1][2];
897  output[33] = result_22[2][0];
898  output[34] = result_22[2][1];
899  output[35] = result_22[2][2];
900 
901  return output;
902 }
903 
910 template <>
911 inline
912 void doTransform(const geometry_msgs::PoseWithCovarianceStamped& t_in, geometry_msgs::PoseWithCovarianceStamped& t_out, const geometry_msgs::TransformStamped& transform)
913 {
914  tf2::Vector3 v;
915  fromMsg(t_in.pose.pose.position, v);
916  tf2::Quaternion r;
917  fromMsg(t_in.pose.pose.orientation, r);
918 
919  tf2::Transform t;
920  fromMsg(transform.transform, t);
921  tf2::Transform v_out = t * tf2::Transform(r, v);
922  toMsg(v_out, t_out.pose.pose);
923  t_out.header.stamp = transform.header.stamp;
924  t_out.header.frame_id = transform.header.frame_id;
925 
926  t_out.pose.covariance = transformCovariance(t_in.pose.covariance, t);
927 }
928 
935 template <>
936 inline
937 void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform)
938  {
939  tf2::Transform input;
940  fromMsg(t_in.transform, input);
941 
942  tf2::Transform t;
943  fromMsg(transform.transform, t);
944  tf2::Transform v_out = t * input;
945 
946  t_out.transform = toMsg(v_out);
947  t_out.header.stamp = transform.header.stamp;
948  t_out.header.frame_id = transform.header.frame_id;
949  }
950 
957 template <>
958 inline
959  void doTransform(const geometry_msgs::Vector3& t_in, geometry_msgs::Vector3& t_out, const geometry_msgs::TransformStamped& transform)
960  {
961  tf2::Transform t;
962  fromMsg(transform.transform, t);
963  tf2::Vector3 v_out = t.getBasis() * tf2::Vector3(t_in.x, t_in.y, t_in.z);
964  t_out.x = v_out[0];
965  t_out.y = v_out[1];
966  t_out.z = v_out[2];
967  }
968 
975 template <>
976 inline
977  void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform)
978  {
979  doTransform(t_in.vector, t_out.vector, transform);
980  t_out.header.stamp = transform.header.stamp;
981  t_out.header.frame_id = transform.header.frame_id;
982  }
983 
984 
985 /**********************/
986 /*** WrenchStamped ****/
987 /**********************/
988 template <>
989 inline
990 const ros::Time& getTimestamp(const geometry_msgs::WrenchStamped& t) {return t.header.stamp;}
991 
992 
993 template <>
994 inline
995 const std::string& getFrameId(const geometry_msgs::WrenchStamped& t) {return t.header.frame_id;}
996 
997 
998 inline
999 geometry_msgs::WrenchStamped toMsg(const geometry_msgs::WrenchStamped& in)
1000 {
1001  return in;
1002 }
1003 
1004 inline
1005 void fromMsg(const geometry_msgs::WrenchStamped& msg, geometry_msgs::WrenchStamped& out)
1006 {
1007  out = msg;
1008 }
1009 
1010 
1011 inline
1012 geometry_msgs::WrenchStamped toMsg(const tf2::Stamped<std::array<tf2::Vector3, 2>>& in, geometry_msgs::WrenchStamped & out)
1013 {
1014  out.header.stamp = in.stamp_;
1015  out.header.frame_id = in.frame_id_;
1016  out.wrench.force = toMsg(in[0]);
1017  out.wrench.torque = toMsg(in[1]);
1018  return out;
1019 }
1020 
1021 
1022 inline
1023 void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped<std::array<tf2::Vector3, 2>>& out)
1024 {
1025  out.stamp_ = msg.header.stamp;
1026  out.frame_id_ = msg.header.frame_id;
1027  tf2::Vector3 tmp;
1028  fromMsg(msg.wrench.force, tmp);
1029  tf2::Vector3 tmp1;
1030  fromMsg(msg.wrench.torque, tmp1);
1031  std::array<tf2::Vector3, 2> tmp_array;
1032  tmp_array[0] = tmp;
1033  tmp_array[1] = tmp1;
1034  out.setData(tmp_array);
1035 }
1036 
1037 template<>
1038 inline
1039 void doTransform(const geometry_msgs::Wrench& t_in, geometry_msgs::Wrench& t_out, const geometry_msgs::TransformStamped& transform)
1040 {
1041  doTransform(t_in.force, t_out.force, transform);
1042  doTransform(t_in.torque, t_out.torque, transform);
1043  // add additional torque created by translating the force
1044  tf2::Vector3 offset = {transform.transform.translation.x, transform.transform.translation.y,
1045  transform.transform.translation.z};
1046  tf2::Vector3 added_torque = offset.cross({t_out.force.x, t_out.force.y, t_out.force.z});
1047  t_out.torque.x += added_torque.getX();
1048  t_out.torque.y += added_torque.getY();
1049  t_out.torque.z += added_torque.getZ();
1050 }
1051 
1052 
1053 template<>
1054 inline
1055 void doTransform(const geometry_msgs::WrenchStamped& t_in, geometry_msgs::WrenchStamped& t_out, const geometry_msgs::TransformStamped& transform)
1056 {
1057  doTransform(t_in.wrench, t_out.wrench, transform);
1058  t_out.header.stamp = transform.header.stamp;
1059  t_out.header.frame_id = transform.header.frame_id;
1060 }
1061 
1062 } // namespace
1063 
1064 #endif // TF2_GEOMETRY_MSGS_H
tf2::getTimestamp
const ros::Time & getTimestamp(const T &t)
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
ROS_DEPRECATED
#define ROS_DEPRECATED
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
tf2::Stamped::stamp_
ros::Time stamp_
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::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:822
ros::Time
tf2::doTransform
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
tf2::getFrameId
const std::string & getFrameId(const T &t)
tf2
tf2::Quaternion
tf2::toMsg
B toMsg(const A &a)
convert.h
tf2::Matrix3x3::transpose
Matrix3x3 transpose() const
tf2::Matrix3x3
macros.h
t
geometry_msgs::TransformStamped t
Definition: test_tf2_geometry_msgs.cpp:41
Transform.h
tf2::Transform::getBasis
TF2SIMD_FORCE_INLINE Matrix3x3 & getBasis()

tf2_geometry_msgs
Author(s): Wim Meeussen
autogenerated on Sun Feb 4 2024 03:18:21