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:30
void add(const MEvent &evt)
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:228
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.
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.
bool sleep() const
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.
bool sleep() const
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.
Connection registerCallback(const C &callback)


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Feb 28 2022 22:26:19