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 
32 #include <tf/message_filter.h>
33 #include <tf/transform_listener.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 
41 #include <gtest/gtest.h>
42 
43 using namespace tf;
44 
46 {
47 public:
48  Notification(int expected_count)
49  : count_(0)
50  , expected_count_(expected_count)
51  , 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_;
68 };
69 
70 TEST(MessageFilter, noTransforms)
71 {
72  tf::TransformListener tf_client;
73  Notification n(1);
74  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
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::now();
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  tf::TransformListener tf_client;
88  Notification n(1);
89  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
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::now();
94  msg->header.frame_id = "frame1";
95  filter.add(msg);
96 
97  EXPECT_EQ(1, n.count_);
98 }
99 
100 TEST(MessageFilter, preexistingTransforms)
101 {
102  tf::TransformListener tf_client;
103  Notification n(1);
104  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
105  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
106 
107  ros::Time stamp = ros::Time::now();
108  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
109  tf_client.setTransform(transform);
110 
111  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
112  msg->header.stamp = stamp;
113  msg->header.frame_id = "frame2";
114 
115  filter.add(msg);
116 
117  EXPECT_EQ(1, n.count_);
118 }
119 
120 TEST(MessageFilter, postTransforms)
121 {
122  tf::TransformListener tf_client;
123  Notification n(1);
124  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
125  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
126 
127  ros::Time stamp = ros::Time::now();
128 
129  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
130  msg->header.stamp = stamp;
131  msg->header.frame_id = "frame2";
132 
133  filter.add(msg);
134 
135  EXPECT_EQ(0, n.count_);
136 
137  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
138  tf_client.setTransform(transform);
139 
140  ros::WallDuration(0.1).sleep();
141  ros::spinOnce();
142 
143  EXPECT_EQ(1, n.count_);
144 }
145 
146 TEST(MessageFilter, queueSize)
147 {
148  tf::TransformListener tf_client;
149  Notification n(10);
150  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 10);
151  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
152  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
153 
154  ros::Time stamp = ros::Time::now();
155 
156  for (int i = 0; i < 20; ++i)
157  {
158  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
159  msg->header.stamp = stamp;
160  msg->header.frame_id = "frame2";
161 
162  filter.add(msg);
163  }
164 
165  EXPECT_EQ(0, n.count_);
166  EXPECT_EQ(10, n.failure_count_);
167 
168  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
169  tf_client.setTransform(transform);
170 
171  ros::WallDuration(0.1).sleep();
172  ros::spinOnce();
173 
174  EXPECT_EQ(10, n.count_);
175 }
176 
177 TEST(MessageFilter, setTargetFrame)
178 {
179  tf::TransformListener tf_client;
180  Notification n(1);
181  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
182  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
183  filter.setTargetFrame("frame1000");
184 
185  ros::Time stamp = ros::Time::now();
186  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2");
187  tf_client.setTransform(transform);
188 
189  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
190  msg->header.stamp = stamp;
191  msg->header.frame_id = "frame2";
192 
193  filter.add(msg);
194 
195 
196  EXPECT_EQ(1, n.count_);
197 }
198 
199 TEST(MessageFilter, multipleTargetFrames)
200 {
201  tf::TransformListener tf_client;
202  Notification n(1);
203  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "", 1);
204  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
205 
206  std::vector<std::string> target_frames;
207  target_frames.push_back("frame1");
208  target_frames.push_back("frame2");
209  filter.setTargetFrames(target_frames);
210 
211  ros::Time stamp = ros::Time::now();
212  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3");
213  tf_client.setTransform(transform);
214 
215  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
216  msg->header.stamp = stamp;
217  msg->header.frame_id = "frame3";
218  filter.add(msg);
219 
220  ros::WallDuration(0.1).sleep();
221  ros::spinOnce();
222 
223  EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
224 
225  //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
226 
227  transform.child_frame_id_ = "frame2";
228  tf_client.setTransform(transform);
229 
230  ros::WallDuration(0.1).sleep();
231  ros::spinOnce();
232 
233  EXPECT_EQ(1, n.count_); // frame2->frame3 now exists
234 }
235 
236 TEST(MessageFilter, tolerance)
237 {
238  ros::Duration offset(0.2);
239  tf::TransformListener tf_client;
240  Notification n(1);
241  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
242  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
243  filter.setTolerance(offset);
244 
245  ros::Time stamp = ros::Time::now();
246  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
247  tf_client.setTransform(transform);
248 
249  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
250  msg->header.stamp = stamp;
251  msg->header.frame_id = "frame2";
252  filter.add(msg);
253 
254  EXPECT_EQ(0, n.count_); //No return due to lack of space for offset
255 
256  //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
257 
258  transform.stamp_ += offset*1.1;
259  tf_client.setTransform(transform);
260 
261  ros::WallDuration(0.1).sleep();
262  ros::spinOnce();
263 
264  EXPECT_EQ(1, n.count_); // Now have data for the message published earlier
265 
266  msg->header.stamp = stamp + offset;
267  filter.add(msg);
268 
269  EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset
270 }
271 
272 // TODO: re-enable once ROS 0.7.3 is out and the Timer issues have been fixed
273 #if 0
274 TEST(MessageFilter, maxRate)
275 {
276  tf::TransformListener tf_client;
277  Notification n(1);
278  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1, ros::NodeHandle(), ros::Duration(1.0));
279  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
280 
281  ros::Time stamp = ros::Time::now();
282  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
283  tf_client.setTransform(transform);
284 
285  stamp += ros::Duration(0.1);
286  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
287  msg->header.stamp = stamp;
288  msg->header.frame_id = "frame2";
289  filter.add(msg);
290 
291  EXPECT_EQ(0, n.count_);
292 
293  transform.stamp_ = stamp;
294  tf_client.setTransform(transform);
295 
296  ros::WallDuration(0.1).sleep();
297  ros::spinOnce();
298 
299  EXPECT_EQ(0, n.count_);
300 
301  //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
302  tf_client.setTransform(transform);
303 
304  ros::WallDuration(0.1).sleep();
305  ros::spinOnce();
306 
307  EXPECT_EQ(1, n.count_);
308 }
309 #endif
310 
311 TEST(MessageFilter, outTheBackFailure)
312 {
313  tf::TransformListener tf_client;
314  Notification n(1);
315  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
316  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
317 
318  ros::Time stamp = ros::Time::now();
319  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
320  tf_client.setTransform(transform);
321 
322  transform.stamp_ = stamp + ros::Duration(10000);
323  tf_client.setTransform(transform);
324 
325  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
326  msg->header.stamp = stamp;
327  msg->header.frame_id = "frame2";
328  filter.add(msg);
329 
330  EXPECT_EQ(1, n.failure_count_);
331 }
332 
333 TEST(MessageFilter, emptyFrameIDFailure)
334 {
335  tf::TransformListener tf_client;
336  Notification n(1);
337  MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
338  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
339 
340  geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
341  msg->header.frame_id = "";
342  filter.add(msg);
343 
344  EXPECT_EQ(1, n.failure_count_);
345 }
346 
347 TEST(MessageFilter, removeCallback)
348 {
349  // Callback queue in separate thread
350  ros::CallbackQueue cbqueue;
351  ros::AsyncSpinner spinner(1, &cbqueue);
352  ros::NodeHandle threaded_nh;
353  threaded_nh.setCallbackQueue(&cbqueue);
354 
355  // TF filters; no data needs to be published
356  boost::scoped_ptr<tf::TransformListener> tf_listener;
357  boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PointStamped> > tf_filter;
358 
359  spinner.start();
360  for (int i = 0; i < 3; ++i) {
361  tf_listener.reset(new tf::TransformListener());
362  // Have callback fire at high rate to increase chances of race condition
363  tf_filter.reset(
365  "map", 5, threaded_nh,
366  ros::Duration(0.000001)));
367 
368  // Sleep and reset; sleeping increases chances of race condition
369  ros::Duration(0.001).sleep();
370  tf_filter.reset();
371  tf_listener.reset();
372  }
373  spinner.stop();
374 }
375 
376 int main(int argc, char** argv)
377 {
378  testing::InitGoogleTest(&argc, argv);
379 
381  ros::init(argc, argv, "test_message_filter");
382  ros::NodeHandle nh;
383 
384  int ret = RUN_ALL_TESTS();
385 
386  return ret;
387 }
TEST(MessageFilter, noTransforms)
int main(int argc, char **argv)
Follows the patterns set by the message_filters package to implement a filter which only passes messa...
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
void add(const MEvent &evt)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
Add transform information to the tf data structure.
Definition: tf.cpp:229
Definition: exceptions.h:38
ros::Time stamp_
The timestamp associated with this transform.
Notification(int expected_count)
static void setNow(const Time &new_now)
void spinner()
std::string child_frame_id_
The frame_id of the coordinate frame this transform defines.
void setTargetFrames(const std::vector< std::string > &target_frames)
Set the frames you need to be able to transform to before getting a message callback.
bool sleep() const
void setCallbackQueue(CallbackQueueInterface *queue)
FilterFailureReason
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
Register a callback to be called when a message is about to be dropped.
void notify(const geometry_msgs::PointStamped::ConstPtr &message)
This class inherits from Transformer and automatically subscribes to ROS transform messages...
static Time now()
void setTargetFrame(const std::string &target_frame)
Set the frame you need to be able to transform to before getting a message callback.
void failure(const geometry_msgs::PointStamped::ConstPtr &message, FilterFailureReason reason)
ROSCPP_DECL void spinOnce()
void setTolerance(const ros::Duration &tolerance)
Set the required tolerance for the notifier to return true.
The Stamped Transform datatype used by tf.
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38
Connection registerCallback(const C &callback)


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sun Jul 8 2018 02:12:41