$search
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Willow Garage nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 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(new 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(MsgConstPtr(new Msg), 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