tf2_geometry_msgs: tf2_geometry

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_ros/buffer.h>
00036 #include <geometry_msgs/PointStamped.h>
00037 #include <geometry_msgs/Vector3Stamped.h>
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <kdl/frames.hpp>
00040 
00041 namespace tf2
00042 {
00043     
00044 KDL::Frame gmTransformToKDL(const geometry_msgs::TransformStamped& t)
00045   {
00046     return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, 
00047                                                 t.transform.rotation.z, t.transform.rotation.w),
00048                       KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
00049   }
00050 
00051 
00052 /********************/
00054 /********************/
00055 
00056 // method to extract timestamp from object
00057 template <>
00058   const ros::Time& getTimestamp(const geometry_msgs::Vector3Stamped& t) {return t.header.stamp;}
00059   
00060 // method to extract frame id from object
00061 template <>
00062   const std::string& getFrameId(const geometry_msgs::Vector3Stamped& t) {return t.header.frame_id;}
00063 
00064 // this method needs to be implemented by client library developers
00065 template <>
00066   void doTransform(const geometry_msgs::Vector3Stamped& t_in, geometry_msgs::Vector3Stamped& t_out, const geometry_msgs::TransformStamped& transform)
00067   {
00068     tf2::Stamped<KDL::Vector> v_out = tf2::Stamped<KDL::Vector>(gmTransformToKDL(transform).M * KDL::Vector(t_in.vector.x, t_in.vector.y, t_in.vector.z), 
00069                                                                 transform.header.stamp, transform.header.frame_id);
00070     t_out.vector.x = v_out[0];
00071     t_out.vector.y = v_out[1];
00072     t_out.vector.z = v_out[2];
00073     t_out.header.stamp = v_out.stamp_;
00074     t_out.header.frame_id = v_out.frame_id_;
00075   }
00076 geometry_msgs::Vector3Stamped toMsg(const geometry_msgs::Vector3Stamped& in)
00077 {
00078   return in;
00079 }
00080 void fromMsg(const geometry_msgs::Vector3Stamped& msg, geometry_msgs::Vector3Stamped& out)
00081 {
00082   out = msg;
00083 }
00084 
00085 
00086 
00087 /******************/
00089 /******************/
00090 
00091 // method to extract timestamp from object
00092 template <>
00093   const ros::Time& getTimestamp(const geometry_msgs::PointStamped& t)  {return t.header.stamp;}
00094 
00095 // method to extract frame id from object
00096 template <>
00097   const std::string& getFrameId(const geometry_msgs::PointStamped& t)  {return t.header.frame_id;}
00098 
00099 // this method needs to be implemented by client library developers
00100 template <>
00101   void doTransform(const geometry_msgs::PointStamped& t_in, geometry_msgs::PointStamped& t_out, const geometry_msgs::TransformStamped& transform)
00102   {
00103     tf2::Stamped<KDL::Vector> v_out = tf2::Stamped<KDL::Vector>(gmTransformToKDL(transform) * KDL::Vector(t_in.point.x, t_in.point.y, t_in.point.z), 
00104                                                                 transform.header.stamp, transform.header.frame_id);
00105     t_out.point.x = v_out[0];
00106     t_out.point.y = v_out[1];
00107     t_out.point.z = v_out[2];
00108     t_out.header.stamp = v_out.stamp_;
00109     t_out.header.frame_id = v_out.frame_id_;
00110   }
00111 geometry_msgs::PointStamped toMsg(const geometry_msgs::PointStamped& in)
00112 {
00113   return in;
00114 }
00115 void fromMsg(const geometry_msgs::PointStamped& msg, geometry_msgs::PointStamped& out)
00116 {
00117   out = msg;
00118 }
00119 
00120 
00121 /*****************/
00123 /*****************/
00124 
00125 // method to extract timestamp from object
00126 template <>
00127   const ros::Time& getTimestamp(const geometry_msgs::PoseStamped& t)  {return t.header.stamp;}
00128 
00129 // method to extract frame id from object
00130 template <>
00131   const std::string& getFrameId(const geometry_msgs::PoseStamped& t)  {return t.header.frame_id;}
00132 
00133 // this method needs to be implemented by client library developers
00134 template <>
00135   void doTransform(const geometry_msgs::PoseStamped& t_in, geometry_msgs::PoseStamped& t_out, const geometry_msgs::TransformStamped& transform)
00136   {
00137     KDL::Vector v(t_in.pose.position.x, t_in.pose.position.y, t_in.pose.position.z);
00138     KDL::Rotation r = KDL::Rotation::Quaternion(t_in.pose.orientation.x, t_in.pose.orientation.y, t_in.pose.orientation.z, t_in.pose.orientation.w);
00139 
00140     tf2::Stamped<KDL::Frame> v_out = tf2::Stamped<KDL::Frame>(gmTransformToKDL(transform) * KDL::Frame(r, v),
00141                                                               transform.header.stamp, transform.header.frame_id);
00142     t_out.pose.position.x = v_out.p[0];
00143     t_out.pose.position.y = v_out.p[1];
00144     t_out.pose.position.z = v_out.p[2];
00145     v_out.M.GetQuaternion(t_out.pose.orientation.x, t_out.pose.orientation.y, t_out.pose.orientation.z, t_out.pose.orientation.w);
00146     t_out.header.stamp = v_out.stamp_;
00147     t_out.header.frame_id = v_out.frame_id_;
00148   }
00149 geometry_msgs::PoseStamped toMsg(const geometry_msgs::PoseStamped& in)
00150 {
00151   return in;
00152 }
00153 void fromMsg(const geometry_msgs::PoseStamped& msg, geometry_msgs::PoseStamped& out)
00154 {
00155   out = msg;
00156 }
00157 
00158 
00159 
00160 
00161 } // namespace
00162 
00163 #endif // TF2_GEOMETRY_MSGS_H
tf2_geometry_msgs.h
