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
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041 #include <string>
00042 #include <gtest/gtest.h>
00043 #include <time.h>
00044 #include <stdlib.h>
00045 #include "ros/ros.h"
00046 #include <rosgraph_msgs/Clock.h>
00047
00048 #include <boost/thread.hpp>
00049 #include <boost/bind.hpp>
00050
00051 int g_argc;
00052 char** g_argv;
00053
00054 class RosClockTest : public testing::Test
00055 {
00056 public:
00057 void setTime(ros::Time t)
00058 {
00059 rosgraph_msgs::Clock message;
00060 message.clock = t;
00061 pub_.publish(message);
00062 }
00063
00064 protected:
00065 RosClockTest()
00066 {
00067 pub_ = nh_.advertise<rosgraph_msgs::Clock>("/clock", 1);
00068 while (pub_.getNumSubscribers() == 0)
00069 {
00070 ros::WallDuration(0.01).sleep();
00071 }
00072 }
00073
00074 ros::NodeHandle nh_;
00075 ros::Publisher pub_;
00076
00077 };
00078
00079 TEST_F(RosClockTest, SimClockTest)
00080 {
00081
00082 ros::Time start = ros::Time::now();
00083
00084
00085 ASSERT_TRUE(start.isZero());
00086
00087
00088 setTime(ros::Time(42, 0));
00089
00090
00091 ros::WallDuration(0.5).sleep();
00092
00093
00094 ASSERT_EQ(42.0, ros::Time::now().toSec());
00095 }
00096
00097 void sleepThread(bool* done)
00098 {
00099 bool ok = ros::Duration(1.0).sleep();
00100 if (!ok)
00101 {
00102 ROS_ERROR("!OK");
00103 }
00104 *done = true;
00105 }
00106
00107 TEST(Clock, sleepFromZero)
00108 {
00109 ros::Time::setNow(ros::Time());
00110 bool done = false;
00111 boost::thread t(boost::bind(sleepThread, &done));
00112
00113 ros::WallDuration(1.0).sleep();
00114 ros::WallTime start = ros::WallTime::now();
00115 ros::Time::setNow(ros::Time(ros::WallTime::now().sec, ros::WallTime::now().nsec));
00116 while (!done)
00117 {
00118 ros::WallDuration(0.001).sleep();
00119 ros::WallTime now = ros::WallTime::now();
00120 ros::Time::setNow(ros::Time(now.sec, now.nsec));
00121 }
00122 ros::WallTime end = ros::WallTime::now();
00123 EXPECT_GE(end - start, ros::WallDuration(1.0));
00124 }
00125
00126 TEST(Clock, isTimeValid)
00127 {
00128 ros::Time::setNow(ros::Time());
00129 ASSERT_FALSE(ros::Time::isValid());
00130 ros::Time::setNow(ros::TIME_MIN);
00131 ASSERT_TRUE(ros::Time::isValid());
00132 }
00133
00134 void waitThread(bool* done)
00135 {
00136 ros::Time::waitForValid();
00137 *done = true;
00138 }
00139
00140 TEST(Clock, waitForValid)
00141 {
00142 ros::Time::setNow(ros::Time());
00143
00144
00145 ros::WallTime start = ros::WallTime::now();
00146 ASSERT_FALSE(ros::Time::waitForValid(ros::WallDuration(1.0)));
00147 ros::WallTime end = ros::WallTime::now();
00148 ASSERT_GT(end - start, ros::WallDuration(1.0));
00149
00150 bool done = false;
00151 boost::thread t(boost::bind(waitThread, &done));
00152
00153 ros::WallDuration(1.0).sleep();
00154 ASSERT_FALSE(done);
00155 ros::Time::setNow(ros::TIME_MIN);
00156 while (!done)
00157 {
00158 ros::WallDuration(0.01).sleep();
00159 }
00160 }
00161
00162 int main(int argc, char** argv)
00163 {
00164 testing::InitGoogleTest(&argc, argv);
00165 ros::init(argc, argv, "sim_time_test");
00166 ros::NodeHandle nh;
00167 g_argc = argc;
00168 g_argv = argv;
00169 return RUN_ALL_TESTS();
00170 }
00171