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 <time.h>
00031 #include <tf_conversions/tf_kdl.h>
00032 #include <gtest/gtest.h>
00033
00034 using namespace tf;
00035
00036 double gen_rand(double min, double max)
00037 {
00038 int rand_num = rand()%100+1;
00039 double result = min + (double)((max-min)*rand_num)/101.0;
00040 return result;
00041 }
00042
00043 TEST(TFKDLConversions, tf_kdl_vector)
00044 {
00045 tf::Vector3 t;
00046 t[0] = gen_rand(-10,10);
00047 t[1] = gen_rand(-10,10);
00048 t[2] = gen_rand(-10,10);
00049
00050 KDL::Vector k;
00051 VectorTFToKDL(t,k);
00052
00053 ASSERT_NEAR(t[0],k[0],1e-6);
00054 ASSERT_NEAR(t[1],k[1],1e-6);
00055 ASSERT_NEAR(t[2],k[2],1e-6);
00056 }
00057
00058 TEST(TFKDLConversions, tf_kdl_rotation)
00059 {
00060 tf::Quaternion t;
00061 t[0] = gen_rand(-1.0,1.0);
00062 t[1] = gen_rand(-1.0,1.0);
00063 t[2] = gen_rand(-1.0,1.0);
00064 t[3] = gen_rand(-1.0,1.0);
00065 t.normalize();
00066
00067 KDL::Rotation k;
00068 QuaternionTFToKDL(t,k);
00069
00070 double x,y,z,w;
00071 k.GetQuaternion(x,y,z,w);
00072
00073 ASSERT_NEAR(fabs(t[0]),fabs(x),1e-6);
00074 ASSERT_NEAR(fabs(t[1]),fabs(y),1e-6);
00075 ASSERT_NEAR(fabs(t[2]),fabs(z),1e-6);
00076 ASSERT_NEAR(fabs(t[3]),fabs(w),1e-6);
00077 ASSERT_NEAR(t[0]*t[1]*t[2]*t[3],x*y*z*w,1e-6);
00078
00079
00080 }
00081
00082 TEST(TFKDLConversions, tf_kdl_transform)
00083 {
00084 tf::Transform t;
00085 tf::Quaternion tq;
00086 tq[0] = gen_rand(-1.0,1.0);
00087 tq[1] = gen_rand(-1.0,1.0);
00088 tq[2] = gen_rand(-1.0,1.0);
00089 tq[3] = gen_rand(-1.0,1.0);
00090 tq.normalize();
00091 t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
00092 t.setRotation(tq);
00093
00094
00095 KDL::Frame k;
00096 TransformTFToKDL(t,k);
00097
00098 for(int i=0; i < 3; i++)
00099 {
00100 ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6);
00101 for(int j=0; j < 3; j++)
00102 {
00103 ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6);
00104 }
00105 }
00106
00107 tf::Transform r;
00108 TransformKDLToTF(k,r);
00109
00110 for(int i=0; i< 3; i++)
00111 {
00112 ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6);
00113 for(int j=0; j < 3; j++)
00114 {
00115 ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6);
00116 }
00117
00118 }
00119 }
00120
00121 TEST(TFKDLConversions, tf_kdl_pose)
00122 {
00123 tf::Pose t;
00124 tf::Quaternion tq;
00125 tq[0] = gen_rand(-1.0,1.0);
00126 tq[1] = gen_rand(-1.0,1.0);
00127 tq[2] = gen_rand(-1.0,1.0);
00128 tq[3] = gen_rand(-1.0,1.0);
00129 tq.normalize();
00130 t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
00131 t.setRotation(tq);
00132
00133
00134 KDL::Frame k;
00135 PoseTFToKDL(t,k);
00136
00137 for(int i=0; i < 3; i++)
00138 {
00139 ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6);
00140 for(int j=0; j < 3; j++)
00141 {
00142 ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6);
00143 }
00144 }
00145
00146
00147 tf::Pose r;
00148 PoseKDLToTF(k,r);
00149
00150 for(int i=0; i< 3; i++)
00151 {
00152 ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6);
00153 for(int j=0; j < 3; j++)
00154 {
00155 ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6);
00156 }
00157
00158 }
00159 }
00160
00161 TEST(TFKDLConversions, msg_kdl_twist)
00162 {
00163 geometry_msgs::Twist m;
00164 m.linear.x = gen_rand(-10,10);
00165 m.linear.y = gen_rand(-10,10);
00166 m.linear.z = gen_rand(-10,10);
00167 m.angular.x = gen_rand(-10,10);
00168 m.angular.y = gen_rand(-10,10);
00169 m.angular.y = gen_rand(-10,10);
00170
00171
00172 KDL::Twist t;
00173 TwistMsgToKDL(m,t);
00174
00175 ASSERT_NEAR(m.linear.x,t.vel[0],1e-6);
00176 ASSERT_NEAR(m.linear.y,t.vel[1],1e-6);
00177 ASSERT_NEAR(m.linear.z,t.vel[2],1e-6);
00178 ASSERT_NEAR(m.angular.x,t.rot[0],1e-6);
00179 ASSERT_NEAR(m.angular.y,t.rot[1],1e-6);
00180 ASSERT_NEAR(m.angular.z,t.rot[2],1e-6);
00181
00182
00183
00184 geometry_msgs::Twist r;
00185 TwistKDLToMsg(t,r);
00186
00187 ASSERT_NEAR(r.linear.x,t.vel[0],1e-6);
00188 ASSERT_NEAR(r.linear.y,t.vel[1],1e-6);
00189 ASSERT_NEAR(r.linear.z,t.vel[2],1e-6);
00190 ASSERT_NEAR(r.angular.x,t.rot[0],1e-6);
00191 ASSERT_NEAR(r.angular.y,t.rot[1],1e-6);
00192 ASSERT_NEAR(r.angular.z,t.rot[2],1e-6);
00193
00194 }
00195
00196
00197
00198 int main(int argc, char **argv){
00199
00200 srand ( time(NULL) );
00201 testing::InitGoogleTest(&argc, argv);
00202 return RUN_ALL_TESTS();
00203 }