Go to the documentation of this file.00001 #include "transform_graph/position.h"
00002
00003 #include <gtest/gtest.h>
00004
00005 #include "geometry_msgs/Point.h"
00006 #include "geometry_msgs/Vector3.h"
00007 #include "pcl/point_types.h"
00008 #include "tf/transform_datatypes.h"
00009 #include "Eigen/Dense"
00010
00011 namespace transform_graph {
00012 TEST(TestPosition, Identity) {
00013 Position p;
00014 EXPECT_EQ(p.vector().x(), 0);
00015 EXPECT_EQ(p.vector().y(), 0);
00016 EXPECT_EQ(p.vector().z(), 0);
00017 }
00018
00019 TEST(TestPosition, TestXyz) {
00020 Position p(1, 2, 3);
00021 EXPECT_EQ(p.vector().x(), 1);
00022 EXPECT_EQ(p.vector().y(), 2);
00023 EXPECT_EQ(p.vector().z(), 3);
00024 }
00025
00026 TEST(TestPosition, TestEigen) {
00027 Eigen::Vector3d v(1, 2, 3);
00028 Position p(v);
00029 EXPECT_EQ(p.vector().x(), 1);
00030 EXPECT_EQ(p.vector().y(), 2);
00031 EXPECT_EQ(p.vector().z(), 3);
00032 }
00033
00034 TEST(TestPosition, TestPoint) {
00035 geometry_msgs::Point p;
00036 p.x = 1;
00037 p.y = 2;
00038 p.z = 3;
00039 Position pos(p);
00040 EXPECT_EQ(pos.vector().x(), 1);
00041 EXPECT_EQ(pos.vector().y(), 2);
00042 EXPECT_EQ(pos.vector().z(), 3);
00043 }
00044
00045 TEST(TestPosition, TestGeoVector) {
00046 geometry_msgs::Vector3 v;
00047 v.x = 1;
00048 v.y = 2;
00049 v.z = 3;
00050 Position pos(v);
00051 EXPECT_EQ(pos.vector().x(), 1);
00052 EXPECT_EQ(pos.vector().y(), 2);
00053 EXPECT_EQ(pos.vector().z(), 3);
00054 }
00055
00056 TEST(TestPosition, TestPclXyz) {
00057 pcl::PointXYZ v;
00058 v.x = 1;
00059 v.y = 2;
00060 v.z = 3;
00061 Position pos(v);
00062 EXPECT_EQ(pos.vector().x(), 1);
00063 EXPECT_EQ(pos.vector().y(), 2);
00064 EXPECT_EQ(pos.vector().z(), 3);
00065 }
00066
00067 TEST(TestPosition, TestTfVector) {
00068 tf::Vector3 v(1, 2, 3);
00069 Position pos(v);
00070 EXPECT_EQ(pos.vector().x(), 1);
00071 EXPECT_EQ(pos.vector().y(), 2);
00072 EXPECT_EQ(pos.vector().z(), 3);
00073 }
00074 }
00075
00076 int main(int argc, char **argv) {
00077 testing::InitGoogleTest(&argc, argv);
00078 return RUN_ALL_TESTS();
00079 }