tf2_kdl: tf2_kdl: tf2_kdl.h Source 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_KDL_H
00033 #define TF2_KDL_H
00034 
00035 #include <tf2_ros/buffer.h>
00036 #include <kdl/frames.hpp>
00037 #include <geometry_msgs/PointStamped.h>
00038 #include <geometry_msgs/TwistStamped.h>
00039 #include <geometry_msgs/WrenchStamped.h>
00040 #include <geometry_msgs/PoseStamped.h>
00041 
00042 
00043 namespace tf2
00044 {
00045     
00046 KDL::Frame transformToKDL(const geometry_msgs::TransformStamped& t)
00047   {
00048     return KDL::Frame(KDL::Rotation::Quaternion(t.transform.rotation.x, t.transform.rotation.y, 
00049                                                 t.transform.rotation.z, t.transform.rotation.w),
00050                       KDL::Vector(t.transform.translation.x, t.transform.translation.y, t.transform.translation.z));
00051   }
00052 
00053 
00054 // ---------------------
00055 // Vector
00056 // ---------------------
00057 // this method needs to be implemented by client library developers
00058 template <>
00059   void doTransform(const tf2::Stamped<KDL::Vector>& t_in, tf2::Stamped<KDL::Vector>& t_out, const geometry_msgs::TransformStamped& transform)
00060   {
00061     t_out = tf2::Stamped<KDL::Vector>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
00062   }
00063 
00064 //convert to vector message
00065 geometry_msgs::PointStamped toMsg(const tf2::Stamped<KDL::Vector>& in)
00066 {
00067   geometry_msgs::PointStamped msg;
00068   msg.header.stamp = in.stamp_;
00069   msg.header.frame_id = in.frame_id_;
00070   msg.point.x = in[0];
00071   msg.point.y = in[1];
00072   msg.point.z = in[2];
00073   return msg;
00074 }
00075 
00076 void fromMsg(const geometry_msgs::PointStamped& msg, tf2::Stamped<KDL::Vector>& out)
00077 {
00078   out.stamp_ = msg.header.stamp;
00079   out.frame_id_ = msg.header.frame_id;
00080   out[0] = msg.point.x;
00081   out[1] = msg.point.y;
00082   out[2] = msg.point.z;
00083 }
00084 
00085 // ---------------------
00086 // Twist
00087 // ---------------------
00088 // this method needs to be implemented by client library developers
00089 template <>
00090   void doTransform(const tf2::Stamped<KDL::Twist>& t_in, tf2::Stamped<KDL::Twist>& t_out, const geometry_msgs::TransformStamped& transform)
00091   {
00092     t_out = tf2::Stamped<KDL::Twist>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
00093   }
00094 
00095 //convert to twist message
00096 geometry_msgs::TwistStamped toMsg(const tf2::Stamped<KDL::Twist>& in)
00097 {
00098   geometry_msgs::TwistStamped msg;
00099   msg.header.stamp = in.stamp_;
00100   msg.header.frame_id = in.frame_id_;
00101   msg.twist.linear.x = in.vel[0];
00102   msg.twist.linear.y = in.vel[1];
00103   msg.twist.linear.z = in.vel[2];
00104   msg.twist.angular.x = in.rot[0];
00105   msg.twist.angular.y = in.rot[1];
00106   msg.twist.angular.z = in.rot[2];
00107   return msg;
00108 }
00109 
00110 void fromMsg(const geometry_msgs::TwistStamped& msg, tf2::Stamped<KDL::Twist>& out)
00111 {
00112   out.stamp_ = msg.header.stamp;
00113   out.frame_id_ = msg.header.frame_id;
00114   out.vel[0] = msg.twist.linear.x;
00115   out.vel[1] = msg.twist.linear.y;
00116   out.vel[2] = msg.twist.linear.z;
00117   out.rot[0] = msg.twist.angular.x;
00118   out.rot[1] = msg.twist.angular.y;
00119   out.rot[2] = msg.twist.angular.z;
00120 }
00121 
00122 
00123 // ---------------------
00124 // Wrench
00125 // ---------------------
00126 // this method needs to be implemented by client library developers
00127 template <>
00128   void doTransform(const tf2::Stamped<KDL::Wrench>& t_in, tf2::Stamped<KDL::Wrench>& t_out, const geometry_msgs::TransformStamped& transform)
00129   {
00130     t_out = tf2::Stamped<KDL::Wrench>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
00131   }
00132 
00133 //convert to wrench message
00134 geometry_msgs::WrenchStamped toMsg(const tf2::Stamped<KDL::Wrench>& in)
00135 {
00136   geometry_msgs::WrenchStamped msg;
00137   msg.header.stamp = in.stamp_;
00138   msg.header.frame_id = in.frame_id_;
00139   msg.wrench.force.x = in.force[0];
00140   msg.wrench.force.y = in.force[1];
00141   msg.wrench.force.z = in.force[2];
00142   msg.wrench.torque.x = in.torque[0];
00143   msg.wrench.torque.y = in.torque[1];
00144   msg.wrench.torque.z = in.torque[2];
00145   return msg;
00146 }
00147 
00148 void fromMsg(const geometry_msgs::WrenchStamped& msg, tf2::Stamped<KDL::Wrench>& out)
00149 {
00150   out.stamp_ = msg.header.stamp;
00151   out.frame_id_ = msg.header.frame_id;
00152   out.force[0] = msg.wrench.force.x;
00153   out.force[1] = msg.wrench.force.y;
00154   out.force[2] = msg.wrench.force.z;
00155   out.torque[0] = msg.wrench.torque.x;
00156   out.torque[1] = msg.wrench.torque.y;
00157   out.torque[2] = msg.wrench.torque.z;
00158 }
00159 
00160 
00161 
00162 
00163 // ---------------------
00164 // Frame
00165 // ---------------------
00166 // this method needs to be implemented by client library developers
00167 template <>
00168   void doTransform(const tf2::Stamped<KDL::Frame>& t_in, tf2::Stamped<KDL::Frame>& t_out, const geometry_msgs::TransformStamped& transform)
00169   {
00170     t_out = tf2::Stamped<KDL::Frame>(transformToKDL(transform) * t_in, transform.header.stamp, transform.header.frame_id);
00171   }
00172 
00173 //convert to pose message
00174 geometry_msgs::PoseStamped toMsg(const tf2::Stamped<KDL::Frame>& in)
00175 {
00176   geometry_msgs::PoseStamped msg;
00177   msg.header.stamp = in.stamp_;
00178   msg.header.frame_id = in.frame_id_;
00179   msg.pose.position.x = in.p[0];
00180   msg.pose.position.y = in.p[1];
00181   msg.pose.position.z = in.p[2];
00182   in.M.GetQuaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w);
00183   return msg;
00184 }
00185 
00186 void fromMsg(const geometry_msgs::PoseStamped& msg, tf2::Stamped<KDL::Frame>& out)
00187 {
00188   out.stamp_ = msg.header.stamp;
00189   out.frame_id_ = msg.header.frame_id;
00190   out.p[0] = msg.pose.position.x;
00191   out.p[1] = msg.pose.position.y;
00192   out.p[2] = msg.pose.position.z;
00193   out.M = KDL::Rotation::Quaternion(msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w);
00194 }
00195 
00196 
00197 } // namespace
00198 
00199 #endif // TF2_KDL_H