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 #include <gtest/gtest.h>
00036
00037 #include "ros/time.h"
00038 #include "message_filters/time_sequencer.h"
00039
00040 using namespace message_filters;
00041
00042 struct Header
00043 {
00044 ros::Time stamp;
00045 };
00046
00047
00048 struct Msg
00049 {
00050 Header header;
00051 int data;
00052 };
00053 typedef boost::shared_ptr<Msg> MsgPtr;
00054 typedef boost::shared_ptr<Msg const> MsgConstPtr;
00055
00056 namespace ros
00057 {
00058 namespace message_traits
00059 {
00060 template<>
00061 struct TimeStamp<Msg>
00062 {
00063 static ros::Time value(const Msg& m)
00064 {
00065 return m.header.stamp;
00066 }
00067 };
00068 }
00069 }
00070
00071 class Helper
00072 {
00073 public:
00074 Helper()
00075 : count_(0)
00076 {}
00077
00078 void cb(const MsgConstPtr&)
00079 {
00080 ++count_;
00081 }
00082
00083 int32_t count_;
00084 };
00085
00086 TEST(TimeSequencer, simple)
00087 {
00088 TimeSequencer<Msg> seq(ros::Duration(1.0), ros::Duration(0.01), 10);
00089 Helper h;
00090 seq.registerCallback(boost::bind(&Helper::cb, &h, _1));
00091 MsgPtr msg(boost::make_shared<Msg>());
00092 msg->header.stamp = ros::Time::now();
00093 seq.add(msg);
00094
00095 ros::WallDuration(0.1).sleep();
00096 ros::spinOnce();
00097 ASSERT_EQ(h.count_, 0);
00098
00099 ros::Time::setNow(ros::Time::now() + ros::Duration(2.0));
00100
00101 ros::WallDuration(0.1).sleep();
00102 ros::spinOnce();
00103
00104 ASSERT_EQ(h.count_, 1);
00105 }
00106
00107 TEST(TimeSequencer, compilation)
00108 {
00109 TimeSequencer<Msg> seq(ros::Duration(1.0), ros::Duration(0.01), 10);
00110 TimeSequencer<Msg> seq2(ros::Duration(1.0), ros::Duration(0.01), 10);
00111 seq2.connectInput(seq);
00112 }
00113
00114 struct EventHelper
00115 {
00116 public:
00117 void cb(const ros::MessageEvent<Msg const>& evt)
00118 {
00119 event_ = evt;
00120 }
00121
00122 ros::MessageEvent<Msg const> event_;
00123 };
00124
00125 TEST(TimeSequencer, eventInEventOut)
00126 {
00127 TimeSequencer<Msg> seq(ros::Duration(1.0), ros::Duration(0.01), 10);
00128 TimeSequencer<Msg> seq2(seq, ros::Duration(1.0), ros::Duration(0.01), 10);
00129 EventHelper h;
00130 seq2.registerCallback(&EventHelper::cb, &h);
00131
00132 ros::MessageEvent<Msg const> evt(boost::make_shared<Msg const>(), ros::Time::now());
00133 seq.add(evt);
00134
00135 ros::Time::setNow(ros::Time::now() + ros::Duration(2));
00136 while (!h.event_.getMessage())
00137 {
00138 ros::WallDuration(0.01).sleep();
00139 ros::spinOnce();
00140 }
00141
00142 EXPECT_EQ(h.event_.getReceiptTime(), evt.getReceiptTime());
00143 EXPECT_EQ(h.event_.getMessage(), evt.getMessage());
00144 }
00145
00146 int main(int argc, char **argv){
00147 testing::InitGoogleTest(&argc, argv);
00148
00149 ros::init(argc, argv, "time_sequencer_test");
00150 ros::NodeHandle nh;
00151 ros::Time::setNow(ros::Time());
00152
00153 return RUN_ALL_TESTS();
00154 }
00155
00156