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_ros/transform_broadcaster.h>
00032 #include <tf2_ros/transform_listener.h>
00033 #include <sys/time.h>
00034 #include <rosgraph_msgs/Clock.h>
00035
00036 using namespace tf2;
00037
00038 TEST(tf2_ros_transform_listener, time_backwards)
00039 {
00040
00041 tf2_ros::Buffer buffer;
00042 tf2_ros::TransformListener tfl(buffer);
00043 tf2_ros::TransformBroadcaster tfb;
00044
00045 ros::NodeHandle nh = ros::NodeHandle();
00046
00047 ros::Publisher clock = nh.advertise<rosgraph_msgs::Clock>("/clock", 5);
00048
00049 rosgraph_msgs::Clock c;
00050 c.clock = ros::Time(100);
00051 clock.publish(c);
00052
00053
00054 ASSERT_FALSE(buffer.canTransform("foo", "bar", ros::Time(101, 0)));
00055
00056
00057 geometry_msgs::TransformStamped msg;
00058 msg.header.stamp = ros::Time(100, 0);
00059 msg.header.frame_id = "foo";
00060 msg.child_frame_id = "bar";
00061 msg.transform.rotation.x = 1.0;
00062 tfb.sendTransform(msg);
00063 msg.header.stamp = ros::Time(102, 0);
00064 tfb.sendTransform(msg);
00065 sleep(1);
00066
00067 ASSERT_TRUE(buffer.canTransform("foo", "bar", ros::Time(101, 0)));
00068
00069 c.clock = ros::Time(90);
00070 clock.publish(c);
00071
00072
00073 msg.header.stamp = ros::Time(110, 0);
00074 msg.header.frame_id = "foo2";
00075 msg.child_frame_id = "bar2";
00076 tfb.sendTransform(msg);
00077 sleep(1);
00078
00079 ASSERT_FALSE(buffer.canTransform("foo", "bar", ros::Time(101, 0)));
00080
00081 }
00082
00083
00084
00085
00086 int main(int argc, char **argv){
00087 testing::InitGoogleTest(&argc, argv);
00088 ros::init(argc, argv, "transform_listener_backwards_reset");
00089 return RUN_ALL_TESTS();
00090 }