tf2_geometry_msgs.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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   // w at the end in the constructor
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 //Backwards compatibility remove when forked for Lunar or newer
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 //Backwards compatibility remove when forked for Lunar or newer
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   // w at the end in the constructor
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   // w at the end in the constructor
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 } // namespace
00741 
00742 #endif // TF2_GEOMETRY_MSGS_H


tf2_geometry_msgs
Author(s): Wim Meeussen
autogenerated on Mon Oct 2 2017 02:24:53