test_message_filter.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
33 #include <tf2_ros/message_filter.h>
34 #include <tf2/buffer_core.h>
35 #include <geometry_msgs/PointStamped.h>
36 #include <boost/bind.hpp>
37 #include <boost/scoped_ptr.hpp>
38 
39 #include "ros/ros.h"
40 #include "ros/callback_queue.h"
41 
42 #include <gtest/gtest.h>
43 
44 using namespace tf2;
45 using namespace tf2_ros;
46 
47 class Notification
48 {
49 public:
50  Notification(int expected_count) :
51  count_(0), expected_count_(expected_count), failure_count_(0)
52  {
53  }
54 
55  void notify(const geometry_msgs::PointStamped::ConstPtr& message)
56  {
57  ++count_;
58  }
59 
60  void failure(const geometry_msgs::PointStamped::ConstPtr& message, FilterFailureReason reason)
61  {
62  ++failure_count_;
63  }
64 
65  int count_;
66  int expected_count_;
67  int failure_count_;
68 };
69 
70 TEST(MessageFilter, noTransforms)
71 {
72  BufferCore bc;
73  Notification n(1);
74  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
75  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
76 
77  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
78  msg->header.stamp = ros::Time(1);
79  msg->header.frame_id = "frame2";
80  filter.add(msg);
81 
82  EXPECT_EQ(0, n.count_);
83 }
84 
85 TEST(MessageFilter, noTransformsSameFrame)
86 {
87  BufferCore bc;
88  Notification n(1);
89  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
90  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
91 
92  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
93  msg->header.stamp = ros::Time(1);
94  msg->header.frame_id = "frame1";
95  filter.add(msg);
96 
97  EXPECT_EQ(1, n.count_);
98 }
99 
100 geometry_msgs::TransformStamped createTransform(Quaternion q, Vector3 v, ros::Time stamp, const std::string& frame1, const std::string& frame2)
101 {
102  geometry_msgs::TransformStamped t;
103  t.header.frame_id = frame1;
104  t.child_frame_id = frame2;
105  t.header.stamp = stamp;
106  t.transform.translation.x = v.x();
107  t.transform.translation.y = v.y();
108  t.transform.translation.z = v.z();
109  t.transform.rotation.x = q.x();
110  t.transform.rotation.y = q.y();
111  t.transform.rotation.z = q.z();
112  t.transform.rotation.w = q.w();
113  return t;
114 }
115 
116 TEST(MessageFilter, preexistingTransforms)
117 {
118  BufferCore bc;
119  Notification n(1);
120  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
121  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
122 
123  ros::Time stamp(1);
124  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
125 
126  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
127  msg->header.stamp = stamp;
128  msg->header.frame_id = "frame2";
129 
130  filter.add(msg);
131 
132  EXPECT_EQ(1, n.count_);
133 }
134 
135 TEST(MessageFilter, postTransforms)
136 {
137  BufferCore bc;
138  Notification n(1);
139  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
140  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
141 
142  ros::Time stamp(1);
143 
144  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
145  msg->header.stamp = stamp;
146  msg->header.frame_id = "frame2";
147 
148  filter.add(msg);
149 
150  EXPECT_EQ(0, n.count_);
151 
152  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
153 
154  EXPECT_EQ(1, n.count_);
155 }
156 
157 TEST(MessageFilter, queueSize)
158 {
159  BufferCore bc;
160  Notification n(10);
161  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 10, 0);
162  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
163  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
164 
165  ros::Time stamp(1);
166 
167  for (int i = 0; i < 20; ++i)
168  {
169  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
170  msg->header.stamp = stamp;
171  msg->header.frame_id = "frame2";
172 
173  filter.add(msg);
174  }
175 
176  EXPECT_EQ(0, n.count_);
177  EXPECT_EQ(10, n.failure_count_);
178 
179  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
180 
181  EXPECT_EQ(10, n.count_);
182 }
183 
184 TEST(MessageFilter, setTargetFrame)
185 {
186  BufferCore bc;
187  Notification n(1);
188  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
189  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
190  filter.setTargetFrame("frame1000");
191 
192  ros::Time stamp(1);
193  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1000", "frame2"), "me");
194 
195  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
196  msg->header.stamp = stamp;
197  msg->header.frame_id = "frame2";
198 
199  filter.add(msg);
200 
201  EXPECT_EQ(1, n.count_);
202 }
203 
204 
205 TEST(MessageFilter, multipleTargetFrames)
206 {
207  BufferCore bc;
208  Notification n(1);
209  MessageFilter<geometry_msgs::PointStamped> filter(bc, "", 1, 0);
210  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
211 
212  std::vector<std::string> target_frames;
213  target_frames.push_back("frame1");
214  target_frames.push_back("frame2");
215  filter.setTargetFrames(target_frames);
216 
217  ros::Time stamp(1);
218  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame3"), "me");
219 
220  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
221  msg->header.stamp = stamp;
222  msg->header.frame_id = "frame3";
223  filter.add(msg);
224 
225  EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
226 
227  //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
228 
229  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
230 
231  EXPECT_EQ(1, n.count_); // frame2->frame3 now exists
232 }
233 
234 TEST(MessageFilter, tolerance)
235 {
236  ros::Duration offset(0.2);
237  BufferCore bc;
238  Notification n(1);
239  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
240  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
241  filter.setTolerance(offset);
242 
243  ros::Time stamp(1);
244  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
245 
246  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
247  msg->header.stamp = stamp;
248  msg->header.frame_id = "frame2";
249  filter.add(msg);
250 
251  EXPECT_EQ(0, n.count_); //No return due to lack of space for offset
252 
253  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + (offset * 1.1), "frame1", "frame2"), "me");
254 
255  EXPECT_EQ(1, n.count_); // Now have data for the message published earlier
256 
257  msg->header.stamp = stamp + offset;
258  filter.add(msg);
259 
260  EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset
261 }
262 
263 TEST(MessageFilter, outTheBackFailure)
264 {
265  BufferCore bc;
266  Notification n(1);
267  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
268  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
269 
270  ros::Time stamp(1);
271  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp, "frame1", "frame2"), "me");
272  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + ros::Duration(10000), "frame1", "frame2"), "me");
273 
274  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
275  msg->header.stamp = stamp;
276  msg->header.frame_id = "frame2";
277  filter.add(msg);
278 
279  EXPECT_EQ(1, n.failure_count_);
280 }
281 
282 TEST(MessageFilter, outTheBackFailure2)
283 {
284  BufferCore bc;
285  Notification n(1);
286  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
287  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
288 
289  ros::Time stamp(1);
290 
291  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
292  msg->header.stamp = stamp;
293  msg->header.frame_id = "frame2";
294  filter.add(msg);
295 
296  EXPECT_EQ(0, n.count_);
297  EXPECT_EQ(0, n.failure_count_);
298 
299  bc.setTransform(createTransform(Quaternion(0,0,0,1), Vector3(1,2,3), stamp + ros::Duration(10000), "frame1", "frame2"), "me");
300 
301  EXPECT_EQ(1, n.failure_count_);
302 }
303 
304 TEST(MessageFilter, emptyFrameIDFailure)
305 {
306  BufferCore bc;
307  Notification n(1);
308  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
309  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
310 
311  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
312  msg->header.frame_id = "";
313  filter.add(msg);
314 
315  EXPECT_EQ(1, n.failure_count_);
316 }
317 
318 TEST(MessageFilter, callbackQueue)
319 {
320  BufferCore bc;
321  Notification n(1);
322  ros::CallbackQueue queue;
323  MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, &queue);
324  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
325 
326  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
327  msg->header.stamp = ros::Time(1);
328  msg->header.frame_id = "frame1";
329  filter.add(msg);
330 
331  EXPECT_EQ(0, n.count_);
332 
333  queue.callAvailable();
334 
335  EXPECT_EQ(1, n.count_);
336 }
337 
338 
339 int main(int argc, char** argv)
340 {
341  testing::InitGoogleTest(&argc, argv);
342 
343  int ret = RUN_ALL_TESTS();
344 
345  return ret;
346 }
TEST
TEST(MessageFilter, noTransforms)
Definition: test_message_filter.cpp:70
main
int main(int argc, char **argv)
Definition: test_message_filter.cpp:339
tf2::BufferCore::setTransform
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
tf2_ros::MessageFilter
tf2_ros::MessageFilter::setTargetFrame
void setTargetFrame(const std::string &target_frame)
buffer_core.h
tf2_ros::MessageFilter::registerFailureCallback
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
t
geometry_msgs::TransformStamped t
tf2_ros::MessageFilter::add
void add(const MEvent &evt)
tf2_ros::MessageFilter::setTargetFrames
void setTargetFrames(const V_string &target_frames)
Vector3
TFSIMD_FORCE_INLINE Vector3()
tf2_ros::MessageFilter::setTolerance
void setTolerance(const ros::Duration &tolerance)
createTransform
geometry_msgs::TransformStamped createTransform(Quaternion q, Vector3 v, ros::Time stamp, const std::string &frame1, const std::string &frame2)
Definition: test_message_filter.cpp:100
Notification::notify
void notify(const geometry_msgs::PointStamped::ConstPtr &message)
FilterFailureReason
FilterFailureReason
message_filter.h
tf2_ros
Notification::failure
void failure(const geometry_msgs::PointStamped::ConstPtr &message, FilterFailureReason reason)
callback_queue.h
message_filters::SimpleFilter::registerCallback
Connection registerCallback(const C &callback)

test_tf2
Author(s): Tully Foote, Eitan Marder-Eppstein
autogenerated on Fri Oct 16 2020 19:09:05