test_tf_message_filter_pcl.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 
34 #include <pcl/point_types.h>
35 #include <pcl/point_cloud.h>
36 
37 #include <tf/message_filter.h>
38 #include <tf/transform_listener.h>
40 
41 #include <boost/bind.hpp>
42 #include <boost/scoped_ptr.hpp>
43 
44 #include <pcl_ros/point_cloud.h>
46 
47 #include <ros/ros.h>
48 
49 #include <gtest/gtest.h>
50 
51 using namespace tf;
52 
53 // using a random point type, as we want to make sure that it does work with
54 // other points than just XYZ
55 typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PCDType;
56 
57 
61 void setStamp(ros::Time &stamp, pcl::uint64_t &pcl_stamp)
62 {
63  // round to millisecond
64  static const uint32_t mult = 1e6;
65  stamp.nsec /= mult;
66  stamp.nsec *= mult;
67 
68  pcl_conversions::toPCL(stamp, pcl_stamp);
69 
70  // verify
71  {
72  ros::Time t;
73  pcl_conversions::fromPCL(pcl_stamp, t);
74  ROS_ASSERT_MSG(t==stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec);
75  }
76 }
77 
78 class Notification
79 {
80 public:
81  Notification(int expected_count)
82  : count_(0)
83  , expected_count_(expected_count)
84  , failure_count_(0)
85  {
86  }
87 
88  void notify(const PCDType::ConstPtr& message)
89  {
90  ++count_;
91  }
92 
93  void failure(const PCDType::ConstPtr& message, FilterFailureReason reason)
94  {
95  ++failure_count_;
96  }
97 
98  int count_;
99  int expected_count_;
100  int failure_count_;
101 };
102 
103 TEST(MessageFilter, noTransforms)
104 {
105  tf::TransformListener tf_client;
106  Notification n(1);
107  MessageFilter<PCDType> filter(tf_client, "frame1", 1);
108  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
109 
110  PCDType::Ptr msg(new PCDType);
111  ros::Time stamp = ros::Time::now();
112  setStamp(stamp, msg->header.stamp);
113  msg->header.frame_id = "frame2";
114  filter.add(msg);
115 
116  EXPECT_EQ(0, n.count_);
117 }
118 
119 TEST(MessageFilter, noTransformsSameFrame)
120 {
121  tf::TransformListener tf_client;
122  Notification n(1);
123  MessageFilter<PCDType> filter(tf_client, "frame1", 1);
124  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
125 
126  PCDType::Ptr msg(new PCDType);
127  ros::Time stamp = ros::Time::now();
128  setStamp(stamp, msg->header.stamp);
129  msg->header.frame_id = "frame1";
130  filter.add(msg);
131 
132  EXPECT_EQ(1, n.count_);
133 }
134 
135 TEST(MessageFilter, preexistingTransforms)
136 {
137  tf::TransformListener tf_client;
138  Notification n(1);
139  MessageFilter<PCDType> filter(tf_client, "frame1", 1);
140  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
141 
142  PCDType::Ptr msg(new PCDType);
143 
144  ros::Time stamp = ros::Time::now();
145  setStamp(stamp, msg->header.stamp);
146 
147  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
148  tf_client.setTransform(transform);
149 
150  msg->header.frame_id = "frame2";
151  filter.add(msg);
152 
153  EXPECT_EQ(1, n.count_);
154 }
155 
156 TEST(MessageFilter, postTransforms)
157 {
158  tf::TransformListener tf_client;
159  Notification n(1);
160  MessageFilter<PCDType> filter(tf_client, "frame1", 1);
161  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
162 
163  ros::Time stamp = ros::Time::now();
164  PCDType::Ptr msg(new PCDType);
165  setStamp(stamp, msg->header.stamp);
166  msg->header.frame_id = "frame2";
167 
168  filter.add(msg);
169 
170  EXPECT_EQ(0, n.count_);
171 
172  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
173  tf_client.setTransform(transform);
174 
175  ros::WallDuration(0.1).sleep();
176  ros::spinOnce();
177 
178  EXPECT_EQ(1, n.count_);
179 }
180 
181 TEST(MessageFilter, queueSize)
182 {
183  tf::TransformListener tf_client;
184  Notification n(10);
185  MessageFilter<PCDType> filter(tf_client, "frame1", 10);
186  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
187  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
188 
189  ros::Time stamp = ros::Time::now();
190  pcl::uint64_t pcl_stamp;
191  setStamp(stamp, pcl_stamp);
192 
193  for (int i = 0; i < 20; ++i)
194  {
195  PCDType::Ptr msg(new PCDType);
196  msg->header.stamp = pcl_stamp;
197  msg->header.frame_id = "frame2";
198 
199  filter.add(msg);
200  }
201 
202  EXPECT_EQ(0, n.count_);
203  EXPECT_EQ(10, n.failure_count_);
204 
205  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
206  tf_client.setTransform(transform);
207 
208  ros::WallDuration(0.1).sleep();
209  ros::spinOnce();
210 
211  EXPECT_EQ(10, n.count_);
212 }
213 
214 TEST(MessageFilter, setTargetFrame)
215 {
216  tf::TransformListener tf_client;
217  Notification n(1);
218  MessageFilter<PCDType> filter(tf_client, "frame1", 1);
219  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
220  filter.setTargetFrame("frame1000");
221 
222  ros::Time stamp = ros::Time::now();
223  PCDType::Ptr msg(new PCDType);
224  setStamp(stamp, msg->header.stamp);
225  msg->header.frame_id = "frame2";
226 
227  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2");
228  tf_client.setTransform(transform);
229 
230  filter.add(msg);
231 
232 
233  EXPECT_EQ(1, n.count_);
234 }
235 
236 TEST(MessageFilter, multipleTargetFrames)
237 {
238  tf::TransformListener tf_client;
239  Notification n(1);
240  MessageFilter<PCDType> filter(tf_client, "", 1);
241  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
242 
243  std::vector<std::string> target_frames;
244  target_frames.push_back("frame1");
245  target_frames.push_back("frame2");
246  filter.setTargetFrames(target_frames);
247 
248  ros::Time stamp = ros::Time::now();
249  PCDType::Ptr msg(new PCDType);
250  setStamp(stamp, msg->header.stamp);
251 
252  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3");
253  tf_client.setTransform(transform);
254 
255  msg->header.frame_id = "frame3";
256  filter.add(msg);
257 
258  ros::WallDuration(0.1).sleep();
259  ros::spinOnce();
260 
261  EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
262 
263  //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
264 
265  transform.child_frame_id_ = "frame2";
266  tf_client.setTransform(transform);
267 
268  ros::WallDuration(0.1).sleep();
269  ros::spinOnce();
270 
271  EXPECT_EQ(1, n.count_); // frame2->frame3 now exists
272 }
273 
274 TEST(MessageFilter, tolerance)
275 {
276  ros::Duration offset(0.2);
277  tf::TransformListener tf_client;
278  Notification n(1);
279  MessageFilter<PCDType> filter(tf_client, "frame1", 1);
280  filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
281  filter.setTolerance(offset);
282 
283  ros::Time stamp = ros::Time::now();
284  pcl::uint64_t pcl_stamp;
285  setStamp(stamp, pcl_stamp);
286  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
287  tf_client.setTransform(transform);
288 
289  PCDType::Ptr msg(new PCDType);
290  msg->header.frame_id = "frame2";
291  msg->header.stamp = pcl_stamp;
292  filter.add(msg);
293 
294  EXPECT_EQ(0, n.count_); //No return due to lack of space for offset
295 
296  //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
297 
298  transform.stamp_ += offset*1.1;
299  tf_client.setTransform(transform);
300 
301  ros::WallDuration(0.1).sleep();
302  ros::spinOnce();
303 
304  EXPECT_EQ(1, n.count_); // Now have data for the message published earlier
305 
306  stamp += offset;
307  setStamp(stamp, pcl_stamp);
308  msg->header.stamp = pcl_stamp;
309 
310  filter.add(msg);
311 
312  EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset
313 }
314 
315 TEST(MessageFilter, outTheBackFailure)
316 {
317  tf::TransformListener tf_client;
318  Notification n(1);
319  MessageFilter<PCDType> filter(tf_client, "frame1", 1);
320  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
321 
322  ros::Time stamp = ros::Time::now();
323  PCDType::Ptr msg(new PCDType);
324  setStamp(stamp, msg->header.stamp);
325 
326  tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
327  tf_client.setTransform(transform);
328 
329  transform.stamp_ = stamp + ros::Duration(10000);
330  tf_client.setTransform(transform);
331 
332  msg->header.frame_id = "frame2";
333  filter.add(msg);
334 
335  EXPECT_EQ(1, n.failure_count_);
336 }
337 
338 TEST(MessageFilter, emptyFrameIDFailure)
339 {
340  tf::TransformListener tf_client;
341  Notification n(1);
342  MessageFilter<PCDType> filter(tf_client, "frame1", 1);
343  filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
344 
345  PCDType::Ptr msg(new PCDType);
346  msg->header.frame_id = "";
347  filter.add(msg);
348 
349  EXPECT_EQ(1, n.failure_count_);
350 }
351 
352 TEST(MessageFilter, removeCallback)
353 {
354  // Callback queue in separate thread
355  ros::CallbackQueue cbqueue;
356  ros::AsyncSpinner spinner(1, &cbqueue);
357  ros::NodeHandle threaded_nh;
358  threaded_nh.setCallbackQueue(&cbqueue);
359 
360  // TF filters; no data needs to be published
361  boost::scoped_ptr<tf::TransformListener> tf_listener;
362  boost::scoped_ptr<tf::MessageFilter<PCDType> > tf_filter;
363 
364  spinner.start();
365  for (int i = 0; i < 3; ++i) {
366  tf_listener.reset(new tf::TransformListener());
367  // Have callback fire at high rate to increase chances of race condition
368  tf_filter.reset(
369  new tf::MessageFilter<PCDType>(*tf_listener,
370  "map", 5, threaded_nh,
371  ros::Duration(0.000001)));
372 
373  // Sleep and reset; sleeping increases chances of race condition
374  ros::Duration(0.001).sleep();
375  tf_filter.reset();
376  tf_listener.reset();
377  }
378  spinner.stop();
379 }
380 
381 int main(int argc, char** argv)
382 {
383  testing::InitGoogleTest(&argc, argv);
384 
386  ros::init(argc, argv, "test_message_filter");
387  ros::NodeHandle nh;
388 
389  int ret = RUN_ALL_TESTS();
390 
391  return ret;
392 }
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")
static void setNow(const Time &new_now)
void spinner()
void setStamp(ros::Time &stamp, pcl::uint64_t &pcl_stamp)
std::string child_frame_id_
void fromPCL(const pcl::uint64_t &pcl_stamp, ros::Time &stamp)
void setTargetFrames(const std::vector< std::string > &target_frames)
bool sleep() const
void setCallbackQueue(CallbackQueueInterface *queue)
#define ROS_ASSERT_MSG(cond,...)
int main(int argc, char **argv)
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
void notify(const geometry_msgs::PointStamped::ConstPtr &message)
FilterFailureReason
static Time now()
void setTargetFrame(const std::string &target_frame)
void failure(const geometry_msgs::PointStamped::ConstPtr &message, FilterFailureReason reason)
ROSCPP_DECL void spinOnce()
void setTolerance(const ros::Duration &tolerance)
void toPCL(const ros::Time &stamp, pcl::uint64_t &pcl_stamp)
TEST(MessageFilter, noTransforms)
Connection registerCallback(const C &callback)
pcl::PointCloud< pcl::PointXYZRGBNormal > PCDType


pcl_ros
Author(s): Open Perception, Julius Kammerl , William Woodall
autogenerated on Mon Jun 10 2019 14:19:18