00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00032 #ifndef TF2_GEOMETRY_MSGS_H
00033 #define TF2_GEOMETRY_MSGS_H
00034
00035 #include <tf2/convert.h>
00036 #include <tf2/LinearMath/Quaternion.h>
00037 #include <tf2/LinearMath/Transform.h>
00038 #include <geometry_msgs/PointStamped.h>
00039 #include <geometry_msgs/QuaternionStamped.h>
00040 #include <geometry_msgs/TransformStamped.h>
00041 #include <geometry_msgs/Vector3Stamped.h>
00042 #include <geometry_msgs/Pose.h>
00043 #include <geometry_msgs/PoseStamped.h>
00044 #include <kdl/frames.hpp>
00045
00046 namespace tf2
00047 {
00048
00054 inline
00055 KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t) __attribute__ ((deprecated));
00056 inline
00057 KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t)
00058 {
00059 return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y,
00060 t.transform.rotation.z, t.transform.rotation.w),
00061 KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
00062 }
00063
00064
00065
00067
00068
00074 inline
00075 geometry_msgs::Vector3 toMsg(const tf2::Vector3& in)
00076 {
00077 geometry_msgs::Vector3 out;
00078 out.x = in.getX();
00079 out.y = in.getY();
00080 out.z = in.getZ();
00081 return out;
00082 }
00083
00089 inline
00090 void fromMsg(const geometry_msgs::Vector3& in, tf2::Vector3& out)
00091 {
00092 out = tf2::Vector3(in.x, in.y, in.z);
00093 }
00094
00095
00096
00098
00099
00106 template <>
00107 inline
00108 const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;}
00109
00116 template <>
00117 inline
00118 const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;}
00119
00120
00126 inline
00127 geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in)
00128 {
00129 return in;
00130 }
00131
00137 inline
00138 void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out)
00139 {
00140 out = msg;
00141 }
00142
00148 inline
00149 geometry_msgs::Vector3Stamped toMsg(const tf2::Stamped<tf2::Vector3>& in)
00150 {
00151 geometry_msgs::Vector3Stamped out;
00152 out.header.stamp = in.stamp_;
00153 out.header.frame_id = in.frame_id_;
00154 out.vector.x = in.getX();
00155 out.vector.y = in.getY();
00156 out.vector.z = in.getZ();
00157 return out;
00158 }
00159
00165 inline
00166 void fromMsg(const geometry_msgs::Vector3Stamped& msg, tf2::Stamped<tf2::Vector3>& out)
00167 {
00168 out.stamp_ = msg.header.stamp;
00169 out.frame_id_ = msg.header.frame_id;
00170 out.setData(tf2::Vector3(msg.vector.x, msg.vector.y, msg.vector.z));
00171 }
00172
00173
00174
00176
00177
00183 inline
00184 geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out)
00185 {
00186 out.x = in.getX();
00187 out.y = in.getY();
00188 out.z = in.getZ();
00189 return out;
00190 }
00191
00197 inline
00198 void fromMsg(const geometry_msgs::Point& in, tf2::Vector3& out)
00199 {
00200 out = tf2::Vector3(in.x, in.y, in.z);
00201 }
00202
00203
00204
00206
00207
00214 template <>
00215 inline
00216 const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t) {return t.header.stamp;}
00217
00224 template <>
00225 inline
00226 const std::string& getFrameId(const geometry_msgs::PointStamped& t) {return t.header.frame_id;}
00227
00233 inline
00234 geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in)
00235 {
00236 return in;
00237 }
00238
00244 inline
00245 void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out)
00246 {
00247 out = msg;
00248 }
00249
00255 inline
00256 geometry_msgs::PointStamped toMsg(const tf2::Stamped<tf2::Vector3>& in, geometry_msgs::PointStamped & out)
00257 {
00258 out.header.stamp = in.stamp_;
00259 out.header.frame_id = in.frame_id_;
00260 out.point.x = in.getX();
00261 out.point.y = in.getY();
00262 out.point.z = in.getZ();
00263 return out;
00264 }
00265
00271 inline
00272 void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<tf2::Vector3>& out)
00273 {
00274 out.stamp_ = msg.header.stamp;
00275 out.frame_id_ = msg.header.frame_id;
00276 out.setData(tf2::Vector3(msg.point.x, msg.point.y, msg.point.z));
00277 }
00278
00279
00280
00282
00283
00289 inline
00290 geometry_msgs::Quaternion toMsg(const tf2::Quaternion& in)
00291 {
00292 geometry_msgs::Quaternion out;
00293 out.w = in.getW();
00294 out.x = in.getX();
00295 out.y = in.getY();
00296 out.z = in.getZ();
00297 return out;
00298 }
00299
00305 inline
00306 void fromMsg(const geometry_msgs::Quaternion& in, tf2::Quaternion& out)
00307 {
00308
00309 out = tf2::Quaternion(in.x, in.y, in.z, in.w);
00310 }
00311
00312
00313
00315
00316
00323 template <>
00324 inline
00325 const ros::Time& getTimestamp(const geometry_msgs::QuaternionStamped& t) {return t.header.stamp;}
00326
00333 template <>
00334 inline
00335 const std::string& getFrameId(const geometry_msgs::QuaternionStamped& t) {return t.header.frame_id;}
00336
00342 inline
00343 geometry_msgs::QuaternionStamped toMsg(const geometry_msgs::QuaternionStamped& in)
00344 {
00345 return in;
00346 }
00347
00353 inline
00354 void fromMsg(const geometry_msgs::QuaternionStamped& msg, geometry_msgs::QuaternionStamped& out)
00355 {
00356 out = msg;
00357 }
00358
00364 inline
00365 geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
00366 {
00367 geometry_msgs::QuaternionStamped out;
00368 out.header.stamp = in.stamp_;
00369 out.header.frame_id = in.frame_id_;
00370 out.quaternion.w = in.getW();
00371 out.quaternion.x = in.getX();
00372 out.quaternion.y = in.getY();
00373 out.quaternion.z = in.getZ();
00374 return out;
00375 }
00376
00377 template <>
00378 inline
00379 geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in) __attribute__ ((deprecated));
00380
00381
00382
00383 template <>
00384 inline
00385 geometry_msgs::QuaternionStamped toMsg(const tf2::Stamped<tf2::Quaternion>& in)
00386 {
00387 return toMsg(in);
00388 }
00389
00395 inline
00396 void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
00397 {
00398 out.stamp_ = in.header.stamp;
00399 out.frame_id_ = in.header.frame_id;
00400 tf2::Quaternion tmp;
00401 fromMsg(in.quaternion, tmp);
00402 out.setData(tmp);
00403 }
00404
00405 template<>
00406 inline
00407 void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out) __attribute__ ((deprecated));
00408
00409
00410 template<>
00411 inline
00412 void fromMsg(const geometry_msgs::QuaternionStamped& in, tf2::Stamped<tf2::Quaternion>& out)
00413 {
00414 fromMsg(in, out);
00415 }
00416
00417
00419
00420
00425 inline
00426 geometry_msgs::Pose& toMsg(const tf2::Transform& in, geometry_msgs::Pose& out)
00427 {
00428 toMsg(in.getOrigin(), out.position);
00429 out.orientation = toMsg(in.getRotation());
00430 return out;
00431 }
00432
00437 inline
00438 void fromMsg(const geometry_msgs::Pose& in, tf2::Transform& out)
00439 {
00440 out.setOrigin(tf2::Vector3(in.position.x, in.position.y, in.position.z));
00441
00442 out.setRotation(tf2::Quaternion(in.orientation.x, in.orientation.y, in.orientation.z, in.orientation.w));
00443 }
00444
00445
00446
00448
00449
00455 template <>
00456 inline
00457 const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t) {return t.header.stamp;}
00458
00464 template <>
00465 inline
00466 const std::string& getFrameId(const geometry_msgs::PoseStamped& t) {return t.header.frame_id;}
00467
00473 inline
00474 geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in)
00475 {
00476 return in;
00477 }
00478
00484 inline
00485 void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out)
00486 {
00487 out = msg;
00488 }
00489
00495 inline
00496 geometry_msgs::PoseStamped toMsg(const tf2::Stamped<tf2::Transform>& in, geometry_msgs::PoseStamped & out)
00497 {
00498 out.header.stamp = in.stamp_;
00499 out.header.frame_id = in.frame_id_;
00500 toMsg(in.getOrigin(), out.pose.position);
00501 out.pose.orientation = toMsg(in.getRotation());
00502 return out;
00503 }
00504
00510 inline
00511 void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<tf2::Transform>& out)
00512 {
00513 out.stamp_ = msg.header.stamp;
00514 out.frame_id_ = msg.header.frame_id;
00515 tf2::Transform tmp;
00516 fromMsg(msg.pose, tmp);
00517 out.setData(tmp);
00518 }
00519
00520
00521
00523
00524
00530 inline
00531 geometry_msgs::Transform toMsg(const tf2::Transform& in)
00532 {
00533 geometry_msgs::Transform out;
00534 out.translation = toMsg(in.getOrigin());
00535 out.rotation = toMsg(in.getRotation());
00536 return out;
00537 }
00538
00544 inline
00545 void fromMsg(const geometry_msgs::Transform& in, tf2::Transform& out)
00546 {
00547 tf2::Vector3 v;
00548 fromMsg(in.translation, v);
00549 out.setOrigin(v);
00550
00551 tf2::Quaternion q;
00552 fromMsg(in.rotation, q);
00553 out.setRotation(q);
00554 }
00555
00556
00557
00559
00560
00566 template <>
00567 inline
00568 const ros::Time& getTimestamp(const geometry_msgs::TransformStamped& t) {return t.header.stamp;}
00569
00575 template <>
00576 inline
00577 const std::string& getFrameId(const geometry_msgs::TransformStamped& t) {return t.header.frame_id;}
00578
00584 inline
00585 geometry_msgs::TransformStamped toMsg(const geometry_msgs::TransformStamped& in)
00586 {
00587 return in;
00588 }
00589
00595 inline
00596 void fromMsg(const geometry_msgs::TransformStamped& msg, geometry_msgs::TransformStamped& out)
00597 {
00598 out = msg;
00599 }
00600
00606 inline
00607 geometry_msgs::TransformStamped toMsg(const tf2::Stamped<tf2::Transform>& in)
00608 {
00609 geometry_msgs::TransformStamped out;
00610 out.header.stamp = in.stamp_;
00611 out.header.frame_id = in.frame_id_;
00612 out.transform.translation = toMsg(in.getOrigin());
00613 out.transform.rotation = toMsg(in.getRotation());
00614 return out;
00615 }
00616
00617
00623 inline
00624 void fromMsg(const geometry_msgs::TransformStamped& msg, tf2::Stamped<tf2::Transform>& out)
00625 {
00626 out.stamp_ = msg.header.stamp;
00627 out.frame_id_ = msg.header.frame_id;
00628 tf2::Transform tmp;
00629 fromMsg(msg.transform, tmp);
00630 out.setData(tmp);
00631 }
00632
00639 template <>
00640 inline
00641 void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform)
00642 {
00643 tf2::Transform t;
00644 fromMsg(transform.transform, t);
00645 tf2::Vector3 v_in;
00646 fromMsg(t_in.point, v_in);
00647 tf2::Vector3 v_out = t * v_in;
00648 toMsg(v_out, t_out.point);
00649 t_out.header.stamp = transform.header.stamp;
00650 t_out.header.frame_id = transform.header.frame_id;
00651 }
00652
00653
00660 template <>
00661 inline
00662 void doTransform(const geometry_msgs::QuaternionStamped& t_in, geometry_msgs::QuaternionStamped& t_out, const geometry_msgs::TransformStamped& transform)
00663 {
00664 tf2::Quaternion t, q_in;
00665 fromMsg(transform.transform.rotation, t);
00666 fromMsg(t_in.quaternion, q_in);
00667
00668 tf2::Quaternion q_out = t * q_in;
00669 t_out.quaternion = toMsg(q_out);
00670 t_out.header.stamp = transform.header.stamp;
00671 t_out.header.frame_id = transform.header.frame_id;
00672 }
00673
00674
00681 template <>
00682 inline
00683 void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform)
00684 {
00685 tf2::Vector3 v;
00686 fromMsg(t_in.pose.position, v);
00687 tf2::Quaternion r;
00688 fromMsg(t_in.pose.orientation, r);
00689
00690 tf2::Transform t;
00691 fromMsg(transform.transform, t);
00692 tf2::Transform v_out = t * tf2::Transform(r, v);
00693 toMsg(v_out, t_out.pose);
00694 t_out.header.stamp = transform.header.stamp;
00695 t_out.header.frame_id = transform.header.frame_id;
00696 }
00697
00704 template <>
00705 inline
00706 void doTransform(const geometry_msgs::TransformStamped& t_in, geometry_msgs::TransformStamped& t_out, const geometry_msgs::TransformStamped& transform)
00707 {
00708 tf2::Transform input;
00709 fromMsg(t_in.transform, input);
00710
00711 tf2::Transform t;
00712 fromMsg(transform.transform, t);
00713 tf2::Transform v_out = t * input;
00714
00715 t_out.transform = toMsg(v_out);
00716 t_out.header.stamp = transform.header.stamp;
00717 t_out.header.frame_id = transform.header.frame_id;
00718 }
00719
00726 template <>
00727 inline
00728 void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform)
00729 {
00730 tf2::Transform t;
00731 fromMsg(transform.transform, t);
00732 tf2::Vector3 v_out = t.getBasis() * tf2::Vector3(t_in.vector.x, t_in.vector.y, t_in.vector.z);
00733 t_out.vector.x = v_out[0];
00734 t_out.vector.y = v_out[1];
00735 t_out.vector.z = v_out[2];
00736 t_out.header.stamp = transform.header.stamp;
00737 t_out.header.frame_id = transform.header.frame_id;
00738 }
00739
00740 }
00741
00742 #endif // TF2_GEOMETRY_MSGS_H