Go to the documentation of this file.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 "kdl_conversions/kdl_msg.h"
00031
00032 namespace tf {
00033
00034 void pointMsgToKDL(const geometry_msgs::Point &m, KDL::Vector &k)
00035 {
00036 k[0] = m.x;
00037 k[1] = m.y;
00038 k[2] = m.z;
00039 }
00040
00041 void pointKDLToMsg(const KDL::Vector &k, geometry_msgs::Point &m)
00042 {
00043 m.x = k[0];
00044 m.y = k[1];
00045 m.z = k[2];
00046 }
00047
00048 void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
00049 {
00050 k.p[0] = m.position.x;
00051 k.p[1] = m.position.y;
00052 k.p[2] = m.position.z;
00053
00054 k.M = KDL::Rotation::Quaternion( m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w);
00055 }
00056
00057 void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
00058 {
00059 m.position.x = k.p[0];
00060 m.position.y = k.p[1];
00061 m.position.z = k.p[2];
00062
00063 k.M.GetQuaternion(m.orientation.x, m.orientation.y, m.orientation.z, m.orientation.w);
00064 }
00065
00066 void quaternionMsgToKDL(const geometry_msgs::Quaternion &m, KDL::Rotation &k)
00067 {
00068 k = KDL::Rotation::Quaternion(m.x, m.y, m.z, m.w);
00069 }
00070
00071 void quaternionKDLToMsg(const KDL::Rotation &k, geometry_msgs::Quaternion &m)
00072 {
00073 k.GetQuaternion(m.x, m.y, m.z, m.w);
00074 }
00075
00076 void transformMsgToKDL(const geometry_msgs::Transform &m, KDL::Frame &k)
00077 {
00078 k.p[0] = m.translation.x;
00079 k.p[1] = m.translation.y;
00080 k.p[2] = m.translation.z;
00081
00082 k.M = KDL::Rotation::Quaternion( m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w);
00083 }
00084
00085 void transformKDLToMsg(const KDL::Frame &k, geometry_msgs::Transform &m)
00086 {
00087 m.translation.x = k.p[0];
00088 m.translation.y = k.p[1];
00089 m.translation.z = k.p[2];
00090
00091 k.M.GetQuaternion(m.rotation.x, m.rotation.y, m.rotation.z, m.rotation.w);
00092 }
00093
00094 void twistKDLToMsg(const KDL::Twist &t, geometry_msgs::Twist &m)
00095 {
00096 m.linear.x = t.vel[0];
00097 m.linear.y = t.vel[1];
00098 m.linear.z = t.vel[2];
00099 m.angular.x = t.rot[0];
00100 m.angular.y = t.rot[1];
00101 m.angular.z = t.rot[2];
00102 }
00103
00104 void twistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &t)
00105 {
00106 t.vel[0] = m.linear.x;
00107 t.vel[1] = m.linear.y;
00108 t.vel[2] = m.linear.z;
00109 t.rot[0] = m.angular.x;
00110 t.rot[1] = m.angular.y;
00111 t.rot[2] = m.angular.z;
00112 }
00113
00114 void vectorMsgToKDL(const geometry_msgs::Vector3 &m, KDL::Vector &k)
00115 {
00116 k[0] = m.x;
00117 k[1] = m.y;
00118 k[2] = m.z;
00119 }
00120
00121 void vectorKDLToMsg(const KDL::Vector &k, geometry_msgs::Vector3 &m)
00122 {
00123 m.x = k[0];
00124 m.y = k[1];
00125 m.z = k[2];
00126 }
00127
00128 void wrenchMsgToKDL(const geometry_msgs::Wrench &m, KDL::Wrench &k)
00129 {
00130 k[0] = m.force.x;
00131 k[1] = m.force.y;
00132 k[2] = m.force.z;
00133 k[3] = m.torque.x;
00134 k[4] = m.torque.y;
00135 k[5] = m.torque.z;
00136 }
00137
00138 void wrenchKDLToMsg(const KDL::Wrench &k, geometry_msgs::Wrench &m)
00139 {
00140 m.force.x = k[0];
00141 m.force.y = k[1];
00142 m.force.z = k[2];
00143 m.torque.x = k[3];
00144 m.torque.y = k[4];
00145 m.torque.z = k[5];
00146 }
00147
00148
00150
00152 void PoseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k) { poseMsgToKDL(m, k);}
00153
00155 void PoseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m) { poseKDLToMsg(k, m);}
00156
00158 void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k) {twistMsgToKDL(m, k);};
00159
00161 void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m){twistKDLToMsg(k, m);};
00162
00163
00164 }
00165