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
00030 #include "tf_conversions/tf_kdl.h"
00031 #include "kdl_conversions/kdl_msg.h"
00032
00033 namespace tf {
00034
00035 void poseTFToKDL(const tf::Pose& t, KDL::Frame& k)
00036 {
00037 for (unsigned int i = 0; i < 3; ++i)
00038 k.p[i] = t.getOrigin()[i];
00039 for (unsigned int i = 0; i < 9; ++i)
00040 k.M.data[i] = t.getBasis()[i/3][i%3];
00041 }
00042
00043 void poseKDLToTF(const KDL::Frame& k, tf::Pose& t)
00044 {
00045 t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
00046 t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
00047 k.M.data[3], k.M.data[4], k.M.data[5],
00048 k.M.data[6], k.M.data[7], k.M.data[8]));
00049 }
00050
00051 void quaternionTFToKDL(const tf::Quaternion& t, KDL::Rotation& k)
00052 {
00053 k = KDL::Rotation::Quaternion(t[0], t[1], t[2], t[3]);
00054 }
00055
00056 void quaternionKDLToTF(const KDL::Rotation &k, tf::Quaternion &t)
00057 {
00058 double x, y, z, w;
00059 k.GetQuaternion(x, y, z, w);
00060 t = tf::Quaternion(x, y, z, w);
00061 }
00062
00063 void transformTFToKDL(const tf::Transform &t, KDL::Frame &k)
00064 {
00065 for (unsigned int i = 0; i < 3; ++i)
00066 k.p[i] = t.getOrigin()[i];
00067 for (unsigned int i = 0; i < 9; ++i)
00068 k.M.data[i] = t.getBasis()[i/3][i%3];
00069 }
00070
00071 void transformKDLToTF(const KDL::Frame &k, tf::Transform &t)
00072 {
00073 t.setOrigin(tf::Vector3(k.p[0], k.p[1], k.p[2]));
00074 t.setBasis(tf::Matrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
00075 k.M.data[3], k.M.data[4], k.M.data[5],
00076 k.M.data[6], k.M.data[7], k.M.data[8]));
00077 }
00078
00079 void vectorTFToKDL(const tf::Vector3& t, KDL::Vector& k)
00080 {
00081 k[0] = t[0];
00082 k[1] = t[1];
00083 k[2] = t[2];
00084 }
00085
00086 void vectorKDLToTF(const KDL::Vector& k, tf::Vector3& t)
00087 {
00088 t[0] = k[0];
00089 t[1] = k[1];
00090 t[2] = k[2];
00091 }
00092
00093
00094
00095 geometry_msgs::Pose addDelta(const geometry_msgs::Pose &pose, const geometry_msgs::Twist &twist, const double &t)
00096 {
00097 geometry_msgs::Pose result;
00098 KDL::Twist kdl_twist;
00099 KDL::Frame kdl_pose_id, kdl_pose;
00100
00101 poseMsgToKDL(pose,kdl_pose);
00102 twistMsgToKDL(twist,kdl_twist);
00103 kdl_pose = KDL::addDelta(kdl_pose_id,kdl_twist,t)*kdl_pose;
00104 poseKDLToMsg(kdl_pose,result);
00105 return result;
00106 }
00107
00108 }
00109