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
00033 #include <tf2_kdl/tf2_kdl.h>
00034 #include <kdl/frames_io.hpp>
00035 #include <gtest/gtest.h>
00036 #include "tf2_ros/buffer.h"
00037
00038
00039 tf2_ros::Buffer* tf_buffer;
00040 static const double EPS = 1e-3;
00041
00042 TEST(TfKDL, Frame)
00043 {
00044 tf2::Stamped<KDL::Frame> v1(KDL::Frame(KDL::Rotation::RPY(M_PI, 0, 0), KDL::Vector(1,2,3)), ros::Time(2.0), "A");
00045
00046
00047
00048 KDL::Frame v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00049 EXPECT_NEAR(v_simple.p[0], -9, EPS);
00050 EXPECT_NEAR(v_simple.p[1], 18, EPS);
00051 EXPECT_NEAR(v_simple.p[2], 27, EPS);
00052 double r, p, y;
00053 v_simple.M.GetRPY(r, p, y);
00054 EXPECT_NEAR(r, 0.0, EPS);
00055 EXPECT_NEAR(p, 0.0, EPS);
00056 EXPECT_NEAR(y, 0.0, EPS);
00057
00058
00059
00060 KDL::Frame v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00061 "A", ros::Duration(3.0));
00062 EXPECT_NEAR(v_advanced.p[0], -9, EPS);
00063 EXPECT_NEAR(v_advanced.p[1], 18, EPS);
00064 EXPECT_NEAR(v_advanced.p[2], 27, EPS);
00065 v_advanced.M.GetRPY(r, p, y);
00066 EXPECT_NEAR(r, 0.0, EPS);
00067 EXPECT_NEAR(p, 0.0, EPS);
00068 EXPECT_NEAR(y, 0.0, EPS);
00069
00070 }
00071
00072
00073
00074 TEST(TfKDL, Vector)
00075 {
00076 tf2::Stamped<KDL::Vector> v1(KDL::Vector(1,2,3), ros::Time(2.0), "A");
00077
00078
00079
00080 KDL::Vector v_simple = tf_buffer->transform(v1, "B", ros::Duration(2.0));
00081 EXPECT_NEAR(v_simple[0], -9, EPS);
00082 EXPECT_NEAR(v_simple[1], 18, EPS);
00083 EXPECT_NEAR(v_simple[2], 27, EPS);
00084
00085
00086 KDL::Vector v_advanced = tf_buffer->transform(v1, "B", ros::Time(2.0),
00087 "A", ros::Duration(3.0));
00088 EXPECT_NEAR(v_advanced[0], -9, EPS);
00089 EXPECT_NEAR(v_advanced[1], 18, EPS);
00090 EXPECT_NEAR(v_advanced[2], 27, EPS);
00091 }
00092
00093 TEST(TfKDL, ConvertVector)
00094 {
00095 tf2::Stamped<KDL::Vector> v(KDL::Vector(1,2,3), ros::Time(), "my_frame");
00096
00097 tf2::Stamped<KDL::Vector> v1 = v;
00098 tf2::convert(v1, v1);
00099
00100 EXPECT_EQ(v, v1);
00101
00102 tf2::Stamped<KDL::Vector> v2(KDL::Vector(3,4,5), ros::Time(), "my_frame2");
00103 tf2::convert(v1, v2);
00104
00105 EXPECT_EQ(v, v2);
00106 EXPECT_EQ(v1, v2);
00107 }
00108
00109
00110 int main(int argc, char **argv){
00111 testing::InitGoogleTest(&argc, argv);
00112 ros::init(argc, argv, "test");
00113 ros::NodeHandle n;
00114
00115 tf_buffer = new tf2_ros::Buffer();
00116
00117
00118 geometry_msgs::TransformStamped t;
00119 t.transform.translation.x = 10;
00120 t.transform.translation.y = 20;
00121 t.transform.translation.z = 30;
00122 t.transform.rotation.x = 1;
00123 t.header.stamp = ros::Time(2.0);
00124 t.header.frame_id = "A";
00125 t.child_frame_id = "B";
00126 tf_buffer->setTransform(t, "test");
00127
00128 int retval = RUN_ALL_TESTS();
00129 delete tf_buffer;
00130 return retval;
00131 }