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
00031
00032
00033
00034
00035
00036
00037 #include <gtest/gtest.h>
00038 #include <tf2/convert.h>
00039 #include <tf2_kdl/tf2_kdl.h>
00040 #include <tf2_bullet/tf2_bullet.h>
00041 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00042 #include <ros/time.h>
00043
00044 TEST(tf2Convert, kdlToBullet)
00045 {
00046 double epsilon = 1e-9;
00047
00048 tf2::Stamped<btVector3> b(btVector3(1,2,3), ros::Time(), "my_frame");
00049
00050 tf2::Stamped<btVector3> b1 = b;
00051 tf2::Stamped<KDL::Vector> k1;
00052 tf2::convert(b1, k1);
00053
00054 tf2::Stamped<btVector3> b2;
00055 tf2::convert(k1, b2);
00056
00057 EXPECT_EQ(b.frame_id_, b2.frame_id_);
00058 EXPECT_NEAR(b.stamp_.toSec(), b2.stamp_.toSec(), epsilon);
00059 EXPECT_NEAR(b.x(), b2.x(), epsilon);
00060 EXPECT_NEAR(b.y(), b2.y(), epsilon);
00061 EXPECT_NEAR(b.z(), b2.z(), epsilon);
00062
00063
00064 EXPECT_EQ(b1.frame_id_, b2.frame_id_);
00065 EXPECT_NEAR(b1.stamp_.toSec(), b2.stamp_.toSec(), epsilon);
00066 EXPECT_NEAR(b1.x(), b2.x(), epsilon);
00067 EXPECT_NEAR(b1.y(), b2.y(), epsilon);
00068 EXPECT_NEAR(b1.z(), b2.z(), epsilon);
00069 }
00070
00071 TEST(tf2Convert, kdlBulletROSConversions)
00072 {
00073 double epsilon = 1e-9;
00074
00075 tf2::Stamped<btVector3> b1(btVector3(1,2,3), ros::Time(), "my_frame"), b2, b3, b4;
00076 geometry_msgs::PointStamped r1, r2, r3;
00077 tf2::Stamped<KDL::Vector> k1, k2, k3;
00078
00079
00080 tf2::convert(b1, b1);
00081 tf2::convert(b1, b2);
00082 tf2::convert(b2, k1);
00083 tf2::convert(k1, k1);
00084 tf2::convert(k1, k2);
00085 tf2::convert(k2, r1);
00086 tf2::convert(r1, r1);
00087 tf2::convert(r1, r2);
00088 tf2::convert(r2, k3);
00089 tf2::convert(k3, b3);
00090 tf2::convert(b3, r3);
00091 tf2::convert(r3, b4);
00092
00093 EXPECT_EQ(b1.frame_id_, b4.frame_id_);
00094 EXPECT_NEAR(b1.stamp_.toSec(), b4.stamp_.toSec(), epsilon);
00095 EXPECT_NEAR(b1.x(), b4.x(), epsilon);
00096 EXPECT_NEAR(b1.y(), b4.y(), epsilon);
00097 EXPECT_NEAR(b1.z(), b4.z(), epsilon);
00098 }
00099
00100 int main(int argc, char** argv)
00101 {
00102 testing::InitGoogleTest(&argc, argv);
00103 return RUN_ALL_TESTS();
00104 }
00105