$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/ros.h" 00038 #include "message_filters/synchronizer.h" 00039 #include "message_filters/sync_policies/exact_time.h" 00040 00041 using namespace message_filters; 00042 using namespace message_filters::sync_policies; 00043 00044 struct Header 00045 { 00046 ros::Time stamp; 00047 }; 00048 00049 00050 struct Msg 00051 { 00052 Header header; 00053 int data; 00054 }; 00055 typedef boost::shared_ptr<Msg> MsgPtr; 00056 typedef boost::shared_ptr<Msg const> MsgConstPtr; 00057 00058 namespace ros 00059 { 00060 namespace message_traits 00061 { 00062 template<> 00063 struct TimeStamp<Msg> 00064 { 00065 static ros::Time value(const Msg& m) 00066 { 00067 return m.header.stamp; 00068 } 00069 }; 00070 } 00071 } 00072 00073 class Helper 00074 { 00075 public: 00076 Helper() 00077 : count_(0) 00078 , drop_count_(0) 00079 {} 00080 00081 void cb() 00082 { 00083 ++count_; 00084 } 00085 00086 void dropcb() 00087 { 00088 ++drop_count_; 00089 } 00090 00091 int32_t count_; 00092 int32_t drop_count_; 00093 }; 00094 00095 typedef ExactTime<Msg, Msg> Policy2; 00096 typedef ExactTime<Msg, Msg, Msg> Policy3; 00097 typedef Synchronizer<Policy2> Sync2; 00098 typedef Synchronizer<Policy3> Sync3; 00099 00101 // From here on we assume that testing the 3-message version is sufficient, so as not to duplicate 00102 // tests for everywhere from 2-9 00104 TEST(ExactTime, multipleTimes) 00105 { 00106 Sync3 sync(2); 00107 Helper h; 00108 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00109 MsgPtr m(new Msg); 00110 m->header.stamp = ros::Time(); 00111 00112 sync.add<0>(m); 00113 ASSERT_EQ(h.count_, 0); 00114 00115 m.reset(new Msg); 00116 m->header.stamp = ros::Time(0.1); 00117 sync.add<1>(m); 00118 ASSERT_EQ(h.count_, 0); 00119 sync.add<0>(m); 00120 ASSERT_EQ(h.count_, 0); 00121 sync.add<2>(m); 00122 ASSERT_EQ(h.count_, 1); 00123 } 00124 00125 TEST(ExactTime, queueSize) 00126 { 00127 Sync3 sync(1); 00128 Helper h; 00129 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00130 MsgPtr m(new Msg); 00131 m->header.stamp = ros::Time(); 00132 00133 sync.add<0>(m); 00134 ASSERT_EQ(h.count_, 0); 00135 sync.add<1>(m); 00136 ASSERT_EQ(h.count_, 0); 00137 00138 m.reset(new Msg); 00139 m->header.stamp = ros::Time(0.1); 00140 sync.add<1>(m); 00141 ASSERT_EQ(h.count_, 0); 00142 00143 m.reset(new Msg); 00144 m->header.stamp = ros::Time(0); 00145 sync.add<1>(m); 00146 ASSERT_EQ(h.count_, 0); 00147 sync.add<2>(m); 00148 ASSERT_EQ(h.count_, 0); 00149 } 00150 00151 TEST(ExactTime, dropCallback) 00152 { 00153 Sync2 sync(1); 00154 Helper h; 00155 sync.registerCallback(boost::bind(&Helper::cb, &h)); 00156 sync.getPolicy()->registerDropCallback(boost::bind(&Helper::dropcb, &h)); 00157 MsgPtr m(new Msg); 00158 m->header.stamp = ros::Time(); 00159 00160 sync.add<0>(m); 00161 ASSERT_EQ(h.drop_count_, 0); 00162 m->header.stamp = ros::Time(0.1); 00163 sync.add<0>(m); 00164 00165 ASSERT_EQ(h.drop_count_, 1); 00166 } 00167 00168 struct EventHelper 00169 { 00170 void callback(const ros::MessageEvent<Msg const>& e1, const ros::MessageEvent<Msg const>& e2) 00171 { 00172 e1_ = e1; 00173 e2_ = e2; 00174 } 00175 00176 ros::MessageEvent<Msg const> e1_; 00177 ros::MessageEvent<Msg const> e2_; 00178 }; 00179 00180 TEST(ExactTime, eventInEventOut) 00181 { 00182 Sync2 sync(2); 00183 EventHelper h; 00184 sync.registerCallback(&EventHelper::callback, &h); 00185 ros::MessageEvent<Msg const> evt(MsgPtr(new Msg), ros::Time(4)); 00186 00187 sync.add<0>(evt); 00188 sync.add<1>(evt); 00189 00190 ASSERT_TRUE(h.e1_.getMessage()); 00191 ASSERT_TRUE(h.e2_.getMessage()); 00192 ASSERT_EQ(h.e1_.getReceiptTime(), evt.getReceiptTime()); 00193 ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime()); 00194 } 00195 00196 int main(int argc, char **argv){ 00197 testing::InitGoogleTest(&argc, argv); 00198 ros::init(argc, argv, "blah"); 00199 00200 ros::Time::init(); 00201 ros::Time::setNow(ros::Time()); 00202 00203 return RUN_ALL_TESTS(); 00204 } 00205 00206