test_exact_time_policy.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 #include <gtest/gtest.h>
36 
37 #include "ros/ros.h"
40 
41 using namespace message_filters;
42 using namespace message_filters::sync_policies;
43 
44 struct Header
45 {
46  ros::Time stamp;
47 };
48 
49 
50 struct Msg
51 {
52  Header header;
53  int data;
54 };
57 
58 namespace ros
59 {
60 namespace message_traits
61 {
62 template<>
63 struct TimeStamp<Msg>
64 {
65  static ros::Time value(const Msg& m)
66  {
67  return m.header.stamp;
68  }
69 };
70 }
71 }
72 
73 class Helper
74 {
75 public:
77  : count_(0)
78  , drop_count_(0)
79  {}
80 
81  void cb()
82  {
83  ++count_;
84  }
85 
86  void dropcb()
87  {
88  ++drop_count_;
89  }
90 
91  int32_t count_;
92  int32_t drop_count_;
93 };
94 
99 
101 // From here on we assume that testing the 3-message version is sufficient, so as not to duplicate
102 // tests for everywhere from 2-9
104 TEST(ExactTime, multipleTimes)
105 {
106  Sync3 sync(2);
107  Helper h;
108  sync.registerCallback(boost::bind(&Helper::cb, &h));
109  MsgPtr m(boost::make_shared<Msg>());
110  m->header.stamp = ros::Time();
111 
112  sync.add<0>(m);
113  ASSERT_EQ(h.count_, 0);
114 
115  m = boost::make_shared<Msg>();
116  m->header.stamp = ros::Time(0.1);
117  sync.add<1>(m);
118  ASSERT_EQ(h.count_, 0);
119  sync.add<0>(m);
120  ASSERT_EQ(h.count_, 0);
121  sync.add<2>(m);
122  ASSERT_EQ(h.count_, 1);
123 }
124 
125 TEST(ExactTime, queueSize)
126 {
127  Sync3 sync(1);
128  Helper h;
129  sync.registerCallback(boost::bind(&Helper::cb, &h));
130  MsgPtr m(boost::make_shared<Msg>());
131  m->header.stamp = ros::Time();
132 
133  sync.add<0>(m);
134  ASSERT_EQ(h.count_, 0);
135  sync.add<1>(m);
136  ASSERT_EQ(h.count_, 0);
137 
138  m = boost::make_shared<Msg>();
139  m->header.stamp = ros::Time(0.1);
140  sync.add<1>(m);
141  ASSERT_EQ(h.count_, 0);
142 
143  m = boost::make_shared<Msg>();
144  m->header.stamp = ros::Time(0);
145  sync.add<1>(m);
146  ASSERT_EQ(h.count_, 0);
147  sync.add<2>(m);
148  ASSERT_EQ(h.count_, 0);
149 }
150 
151 TEST(ExactTime, dropCallback)
152 {
153  Sync2 sync(1);
154  Helper h;
155  sync.registerCallback(boost::bind(&Helper::cb, &h));
156  sync.getPolicy()->registerDropCallback(boost::bind(&Helper::dropcb, &h));
157  MsgPtr m(boost::make_shared<Msg>());
158  m->header.stamp = ros::Time();
159 
160  sync.add<0>(m);
161  ASSERT_EQ(h.drop_count_, 0);
162  m->header.stamp = ros::Time(0.1);
163  sync.add<0>(m);
164 
165  ASSERT_EQ(h.drop_count_, 1);
166 }
167 
168 struct EventHelper
169 {
171  {
172  e1_ = e1;
173  e2_ = e2;
174  }
175 
178 };
179 
180 TEST(ExactTime, eventInEventOut)
181 {
182  Sync2 sync(2);
183  EventHelper h;
185  ros::MessageEvent<Msg const> evt(boost::make_shared<Msg>(), ros::Time(4));
186 
187  sync.add<0>(evt);
188  sync.add<1>(evt);
189 
190  ASSERT_TRUE(h.e1_.getMessage());
191  ASSERT_TRUE(h.e2_.getMessage());
192  ASSERT_EQ(h.e1_.getReceiptTime(), evt.getReceiptTime());
193  ASSERT_EQ(h.e2_.getReceiptTime(), evt.getReceiptTime());
194 }
195 
196 int main(int argc, char **argv){
197  testing::InitGoogleTest(&argc, argv);
198  ros::init(argc, argv, "blah");
199 
200  ros::Time::init();
202 
203  return RUN_ALL_TESTS();
204 }
205 
206 
boost::shared_ptr< Msg const > MsgConstPtr
ros::Time getReceiptTime() const
Header header
void add(const boost::shared_ptr< typename mpl::at_c< Messages, i >::type const > &msg)
Definition: synchronizer.h:340
int32_t count_
Definition: test_chain.cpp:61
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std_msgs::Header * header(M &m)
static void setNow(const Time &new_now)
void callback(const ros::MessageEvent< Msg const > &e1, const ros::MessageEvent< Msg const > &e2)
Connection registerCallback(C &callback)
Definition: synchronizer.h:302
ros::MessageEvent< Msg const > e1_
static void init()
ExactTime< Msg, Msg > Policy2
TEST(ExactTime, multipleTimes)
boost::shared_ptr< Msg > MsgPtr
Synchronizer< Policy2 > Sync2
ros::MessageEvent< Msg const > e2_
Synchronizer< Policy3 > Sync3
ExactTime< Msg, Msg, Msg > Policy3
int main(int argc, char **argv)
boost::shared_ptr< M > getMessage() const
ros::Time stamp


message_filters
Author(s): Josh Faust, Vijay Pradeep, Dirk Thomas
autogenerated on Mon Nov 2 2020 03:52:42