Go to the documentation of this file.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 <gtest/gtest.h>
00031 #include <tf2/time_cache.h>
00032 #include <sys/time.h>
00033 #include "LinearMath/btVector3.h"
00034 #include "LinearMath/btMatrix3x3.h"
00035 #include <stdexcept>
00036
00037 #include <geometry_msgs/TransformStamped.h>
00038
00039 #include <cmath>
00040
00041 using namespace tf2;
00042
00043
00044 void setIdentity(TransformStorage& stor)
00045 {
00046 stor.translation_.setValue(0.0, 0.0, 0.0);
00047 stor.rotation_.setValue(0.0, 0.0, 0.0, 1.0);
00048 }
00049
00050 TEST(StaticCache, Repeatability)
00051 {
00052 unsigned int runs = 100;
00053
00054 tf2::StaticCache cache;
00055
00056 TransformStorage stor;
00057 setIdentity(stor);
00058
00059 for ( uint64_t i = 1; i < runs ; i++ )
00060 {
00061 stor.frame_id_ = CompactFrameID(i);
00062 stor.stamp_ = ros::Time().fromNSec(i);
00063
00064 cache.insertData(stor);
00065
00066
00067 cache.getData(ros::Time().fromNSec(i), stor);
00068 EXPECT_EQ(stor.frame_id_, i);
00069 EXPECT_EQ(stor.stamp_, ros::Time().fromNSec(i));
00070
00071 }
00072 }
00073
00074 TEST(StaticCache, DuplicateEntries)
00075 {
00076
00077 tf2::StaticCache cache;
00078
00079 TransformStorage stor;
00080 setIdentity(stor);
00081 stor.frame_id_ = CompactFrameID(3);
00082 stor.stamp_ = ros::Time().fromNSec(1);
00083
00084 cache.insertData(stor);
00085
00086 cache.insertData(stor);
00087
00088
00089 cache.getData(ros::Time().fromNSec(1), stor);
00090
00091
00092 EXPECT_TRUE(!std::isnan(stor.translation_.x()));
00093 EXPECT_TRUE(!std::isnan(stor.translation_.y()));
00094 EXPECT_TRUE(!std::isnan(stor.translation_.z()));
00095 EXPECT_TRUE(!std::isnan(stor.rotation_.x()));
00096 EXPECT_TRUE(!std::isnan(stor.rotation_.y()));
00097 EXPECT_TRUE(!std::isnan(stor.rotation_.z()));
00098 EXPECT_TRUE(!std::isnan(stor.rotation_.w()));
00099 }
00100
00101 int main(int argc, char **argv){
00102 testing::InitGoogleTest(&argc, argv);
00103 return RUN_ALL_TESTS();
00104 }