test_kdl_tf.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <time.h>
31 #include <tf_conversions/tf_kdl.h>
32 #include <gtest/gtest.h>
33 
34 using namespace tf;
35 
36 double gen_rand(double min, double max)
37 {
38  int rand_num = rand()%100+1;
39  double result = min + (double)((max-min)*rand_num)/101.0;
40  return result;
41 }
42 
43 TEST(TFKDLConversions, tf_kdl_vector)
44 {
45  tf::Vector3 t;
46  t[0] = gen_rand(-10,10);
47  t[1] = gen_rand(-10,10);
48  t[2] = gen_rand(-10,10);
49 
50  KDL::Vector k;
51  VectorTFToKDL(t,k);
52 
53  ASSERT_NEAR(t[0],k[0],1e-6);
54  ASSERT_NEAR(t[1],k[1],1e-6);
55  ASSERT_NEAR(t[2],k[2],1e-6);
56 }
57 
58 TEST(TFKDLConversions, tf_kdl_rotation)
59 {
61  t[0] = gen_rand(-1.0,1.0);
62  t[1] = gen_rand(-1.0,1.0);
63  t[2] = gen_rand(-1.0,1.0);
64  t[3] = gen_rand(-1.0,1.0);
65  t.normalize();
66 
67  KDL::Rotation k;
68  QuaternionTFToKDL(t,k);
69 
70  double x,y,z,w;
71  k.GetQuaternion(x,y,z,w);
72 
73  ASSERT_NEAR(fabs(t[0]),fabs(x),1e-6);
74  ASSERT_NEAR(fabs(t[1]),fabs(y),1e-6);
75  ASSERT_NEAR(fabs(t[2]),fabs(z),1e-6);
76  ASSERT_NEAR(fabs(t[3]),fabs(w),1e-6);
77  ASSERT_NEAR(t[0]*t[1]*t[2]*t[3],x*y*z*w,1e-6);
78 
79 
80 }
81 
82 TEST(TFKDLConversions, tf_kdl_transform)
83 {
84  tf::Transform t;
85  tf::Quaternion tq;
86  tq[0] = gen_rand(-1.0,1.0);
87  tq[1] = gen_rand(-1.0,1.0);
88  tq[2] = gen_rand(-1.0,1.0);
89  tq[3] = gen_rand(-1.0,1.0);
90  tq.normalize();
91  t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
92  t.setRotation(tq);
93 
94  //test tf->kdl
95  KDL::Frame k;
96  TransformTFToKDL(t,k);
97 
98  for(int i=0; i < 3; i++)
99  {
100  ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6);
101  for(int j=0; j < 3; j++)
102  {
103  ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6);
104  }
105  }
106  //test kdl->tf
107  tf::Transform r;
108  TransformKDLToTF(k,r);
109 
110  for(int i=0; i< 3; i++)
111  {
112  ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6);
113  for(int j=0; j < 3; j++)
114  {
115  ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6);
116  }
117 
118  }
119 }
120 
121 TEST(TFKDLConversions, tf_kdl_pose)
122 {
123  tf::Pose t;
124  tf::Quaternion tq;
125  tq[0] = gen_rand(-1.0,1.0);
126  tq[1] = gen_rand(-1.0,1.0);
127  tq[2] = gen_rand(-1.0,1.0);
128  tq[3] = gen_rand(-1.0,1.0);
129  tq.normalize();
130  t.setOrigin(tf::Vector3(gen_rand(-10,10),gen_rand(-10,10),gen_rand(-10,10)));
131  t.setRotation(tq);
132 
133  //test tf->kdl
134  KDL::Frame k;
135  PoseTFToKDL(t,k);
136 
137  for(int i=0; i < 3; i++)
138  {
139  ASSERT_NEAR(t.getOrigin()[i],k.p[i],1e-6);
140  for(int j=0; j < 3; j++)
141  {
142  ASSERT_NEAR(t.getBasis()[i][j],k.M(i,j),1e-6);
143  }
144  }
145 
146  //test kdl->tf
147  tf::Pose r;
148  PoseKDLToTF(k,r);
149 
150  for(int i=0; i< 3; i++)
151  {
152  ASSERT_NEAR(t.getOrigin()[i],r.getOrigin()[i],1e-6);
153  for(int j=0; j < 3; j++)
154  {
155  ASSERT_NEAR(t.getBasis()[i][j],r.getBasis()[i][j],1e-6);
156  }
157 
158  }
159 }
160 
161 TEST(TFKDLConversions, msg_kdl_twist)
162 {
163  geometry_msgs::Twist m;
164  m.linear.x = gen_rand(-10,10);
165  m.linear.y = gen_rand(-10,10);
166  m.linear.z = gen_rand(-10,10);
167  m.angular.x = gen_rand(-10,10);
168  m.angular.y = gen_rand(-10,10);
169  m.angular.y = gen_rand(-10,10);
170 
171  //test msg->kdl
172  KDL::Twist t;
173  TwistMsgToKDL(m,t);
174 
175  ASSERT_NEAR(m.linear.x,t.vel[0],1e-6);
176  ASSERT_NEAR(m.linear.y,t.vel[1],1e-6);
177  ASSERT_NEAR(m.linear.z,t.vel[2],1e-6);
178  ASSERT_NEAR(m.angular.x,t.rot[0],1e-6);
179  ASSERT_NEAR(m.angular.y,t.rot[1],1e-6);
180  ASSERT_NEAR(m.angular.z,t.rot[2],1e-6);
181 
182 
183  //test msg->kdl
184  geometry_msgs::Twist r;
185  TwistKDLToMsg(t,r);
186 
187  ASSERT_NEAR(r.linear.x,t.vel[0],1e-6);
188  ASSERT_NEAR(r.linear.y,t.vel[1],1e-6);
189  ASSERT_NEAR(r.linear.z,t.vel[2],1e-6);
190  ASSERT_NEAR(r.angular.x,t.rot[0],1e-6);
191  ASSERT_NEAR(r.angular.y,t.rot[1],1e-6);
192  ASSERT_NEAR(r.angular.z,t.rot[2],1e-6);
193 
194 }
195 
196 
197 
198 int main(int argc, char **argv){
199 /* initialize random seed: */
200  srand ( time(NULL) );
201  testing::InitGoogleTest(&argc, argv);
202  return RUN_ALL_TESTS();
203 }
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
void TransformTFToKDL(const tf::Transform &t, KDL::Frame &k) __attribute__((deprecated))
Converts a tf Transform into a KDL Frame.
Definition: tf_kdl.h:90
TEST(TFKDLConversions, tf_kdl_vector)
Definition: test_kdl_tf.cpp:43
void TransformKDLToTF(const KDL::Frame &k, tf::Transform &t) __attribute__((deprecated))
Converts a KDL Frame into a tf Transform.
Definition: tf_kdl.h:94
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
Vector vel
void PoseKDLToTF(const KDL::Frame &k, tf::Pose &t) __attribute__((deprecated))
Converts a KDL Frame into a tf Pose.
Definition: tf_kdl.h:78
Quaternion & normalize()
void VectorTFToKDL(const tf::Vector3 &t, KDL::Vector &k) __attribute__((deprecated))
Converts a tf Vector3 into a KDL Vector.
Definition: tf_kdl.h:98
void QuaternionTFToKDL(const tf::Quaternion &t, KDL::Rotation &k) __attribute__((deprecated))
Converts a tf Quaternion into a KDL Rotation.
Definition: tf_kdl.h:82
TFSIMD_FORCE_INLINE const tfScalar & y() const
void PoseTFToKDL(const tf::Pose &t, KDL::Frame &k) __attribute__((deprecated))
Converts a tf Pose into a KDL Frame.
Definition: tf_kdl.h:74
Rotation M
void GetQuaternion(double &x, double &y, double &z, double &w) const
void TwistMsgToKDL(const geometry_msgs::Twist &m, KDL::Twist &k) __attribute__((deprecated))
Vector rot
TFSIMD_FORCE_INLINE const tfScalar & x() const
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
int main(int argc, char **argv)
TFSIMD_FORCE_INLINE const tfScalar & z() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
double gen_rand(double min, double max)
Definition: test_kdl_tf.cpp:36
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
void TwistKDLToMsg(const KDL::Twist &k, geometry_msgs::Twist &m) __attribute__((deprecated))


tf_conversions
Author(s): Tully Foote
autogenerated on Fri Jun 7 2019 22:00:41