test_message_filter.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00032 #include <tf/message_filter.h>
00033 #include <tf/transform_listener.h>
00034 #include <tf/transform_broadcaster.h>
00035 #include <geometry_msgs/PointStamped.h>
00036 #include <boost/bind.hpp>
00037 #include <boost/scoped_ptr.hpp>
00038 
00039 #include "ros/ros.h"
00040 
00041 #include <gtest/gtest.h>
00042 
00043 using namespace tf;
00044 
00045 class Notification
00046 {
00047 public:
00048         Notification(int expected_count)
00049         : count_(0)
00050         , expected_count_(expected_count)
00051         , failure_count_(0)
00052         {
00053         }
00054 
00055         void notify(const geometry_msgs::PointStamped::ConstPtr& message)
00056         {
00057                 ++count_;
00058         }
00059 
00060         void failure(const geometry_msgs::PointStamped::ConstPtr& message, FilterFailureReason reason)
00061         {
00062           ++failure_count_;
00063         }
00064 
00065         int count_;
00066         int expected_count_;
00067         int failure_count_;
00068 };
00069 
00070 TEST(MessageFilter, noTransforms)
00071 {
00072   tf::TransformListener tf_client;
00073         Notification n(1);
00074         MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
00075         filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00076 
00077         geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00078         msg->header.stamp = ros::Time::now();
00079         msg->header.frame_id = "frame2";
00080         filter.add(msg);
00081 
00082         EXPECT_EQ(0, n.count_);
00083 }
00084 
00085 TEST(MessageFilter, noTransformsSameFrame)
00086 {
00087   tf::TransformListener tf_client;
00088   Notification n(1);
00089   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
00090   filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00091 
00092   geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00093   msg->header.stamp = ros::Time::now();
00094   msg->header.frame_id = "frame1";
00095   filter.add(msg);
00096 
00097   EXPECT_EQ(1, n.count_);
00098 }
00099 
00100 TEST(MessageFilter, preexistingTransforms)
00101 {
00102   tf::TransformListener tf_client;
00103   Notification n(1);
00104   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
00105   filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00106 
00107         ros::Time stamp = ros::Time::now();
00108         tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00109         tf_client.setTransform(transform);
00110 
00111         geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00112         msg->header.stamp = stamp;
00113         msg->header.frame_id = "frame2";
00114 
00115         filter.add(msg);
00116 
00117         EXPECT_EQ(1, n.count_);
00118 }
00119 
00120 TEST(MessageFilter, postTransforms)
00121 {
00122   tf::TransformListener tf_client;
00123   Notification n(1);
00124   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
00125   filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00126 
00127         ros::Time stamp = ros::Time::now();
00128 
00129         geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00130   msg->header.stamp = stamp;
00131   msg->header.frame_id = "frame2";
00132 
00133   filter.add(msg);
00134 
00135         EXPECT_EQ(0, n.count_);
00136 
00137         tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00138         tf_client.setTransform(transform);
00139 
00140         ros::WallDuration(0.1).sleep();
00141         ros::spinOnce();
00142 
00143         EXPECT_EQ(1, n.count_);
00144 }
00145 
00146 TEST(MessageFilter, queueSize)
00147 {
00148   tf::TransformListener tf_client;
00149   Notification n(10);
00150   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 10);
00151   filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00152   filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00153 
00154         ros::Time stamp = ros::Time::now();
00155 
00156         for (int i = 0; i < 20; ++i)
00157         {
00158           geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00159     msg->header.stamp = stamp;
00160     msg->header.frame_id = "frame2";
00161 
00162     filter.add(msg);
00163         }
00164 
00165         EXPECT_EQ(0, n.count_);
00166         EXPECT_EQ(10, n.failure_count_);
00167 
00168         tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00169         tf_client.setTransform(transform);
00170 
00171         ros::WallDuration(0.1).sleep();
00172         ros::spinOnce();
00173 
00174         EXPECT_EQ(10, n.count_);
00175 }
00176 
00177 TEST(MessageFilter, setTargetFrame)
00178 {
00179   tf::TransformListener tf_client;
00180   Notification n(1);
00181   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
00182   filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00183         filter.setTargetFrame("frame1000");
00184 
00185         ros::Time stamp = ros::Time::now();
00186   tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2");
00187   tf_client.setTransform(transform);
00188 
00189   geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00190   msg->header.stamp = stamp;
00191   msg->header.frame_id = "frame2";
00192 
00193         filter.add(msg);
00194 
00195 
00196         EXPECT_EQ(1, n.count_);
00197 }
00198 
00199 TEST(MessageFilter, multipleTargetFrames)
00200 {
00201   tf::TransformListener tf_client;
00202   Notification n(1);
00203   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "", 1);
00204   filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00205 
00206   std::vector<std::string> target_frames;
00207   target_frames.push_back("frame1");
00208   target_frames.push_back("frame2");
00209         filter.setTargetFrames(target_frames);
00210 
00211         ros::Time stamp = ros::Time::now();
00212   tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3");
00213   tf_client.setTransform(transform);
00214 
00215   geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00216   msg->header.stamp = stamp;
00217   msg->header.frame_id = "frame3";
00218   filter.add(msg);
00219 
00220   ros::WallDuration(0.1).sleep();
00221   ros::spinOnce();
00222 
00223         EXPECT_EQ(0, n.count_); // frame1->frame3 exists, frame2->frame3 does not (yet)
00224 
00225         //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
00226 
00227         transform.child_frame_id_ = "frame2";
00228         tf_client.setTransform(transform);
00229 
00230         ros::WallDuration(0.1).sleep();
00231         ros::spinOnce();
00232 
00233         EXPECT_EQ(1, n.count_);  // frame2->frame3 now exists
00234 }
00235 
00236 TEST(MessageFilter, tolerance)
00237 {
00238   ros::Duration offset(0.2);
00239   tf::TransformListener tf_client;
00240   Notification n(1);
00241   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
00242   filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00243   filter.setTolerance(offset);
00244 
00245         ros::Time stamp = ros::Time::now();
00246   tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00247   tf_client.setTransform(transform);
00248 
00249   geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00250   msg->header.stamp = stamp;
00251   msg->header.frame_id = "frame2";
00252   filter.add(msg);
00253 
00254         EXPECT_EQ(0, n.count_); //No return due to lack of space for offset
00255 
00256         //ros::Time::setNow(ros::Time::now() + ros::Duration(0.1));
00257 
00258         transform.stamp_ += offset*1.1;
00259         tf_client.setTransform(transform);
00260 
00261         ros::WallDuration(0.1).sleep();
00262         ros::spinOnce();
00263 
00264         EXPECT_EQ(1, n.count_); // Now have data for the message published earlier
00265 
00266         msg->header.stamp = stamp + offset;
00267         filter.add(msg);
00268 
00269         EXPECT_EQ(1, n.count_); // Latest message is off the end of the offset
00270 }
00271 
00272 // TODO: re-enable once ROS 0.7.3 is out and the Timer issues have been fixed
00273 #if 0
00274 TEST(MessageFilter, maxRate)
00275 {
00276   tf::TransformListener tf_client;
00277   Notification n(1);
00278   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1, ros::NodeHandle(), ros::Duration(1.0));
00279   filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00280 
00281   ros::Time stamp = ros::Time::now();
00282   tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00283   tf_client.setTransform(transform);
00284 
00285   stamp += ros::Duration(0.1);
00286   geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00287   msg->header.stamp = stamp;
00288   msg->header.frame_id = "frame2";
00289   filter.add(msg);
00290 
00291   EXPECT_EQ(0, n.count_);
00292 
00293   transform.stamp_ = stamp;
00294   tf_client.setTransform(transform);
00295 
00296   ros::WallDuration(0.1).sleep();
00297   ros::spinOnce();
00298 
00299   EXPECT_EQ(0, n.count_);
00300 
00301   //ros::Time::setNow(ros::Time::now() + ros::Duration(1.0));
00302   tf_client.setTransform(transform);
00303 
00304   ros::WallDuration(0.1).sleep();
00305   ros::spinOnce();
00306 
00307   EXPECT_EQ(1, n.count_);
00308 }
00309 #endif
00310 
00311 TEST(MessageFilter, outTheBackFailure)
00312 {
00313   tf::TransformListener tf_client;
00314   Notification n(1);
00315   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
00316   filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00317 
00318   ros::Time stamp = ros::Time::now();
00319   tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00320   tf_client.setTransform(transform);
00321 
00322   transform.stamp_ = stamp + ros::Duration(10000);
00323   tf_client.setTransform(transform);
00324 
00325   geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00326   msg->header.stamp = stamp;
00327   msg->header.frame_id = "frame2";
00328   filter.add(msg);
00329 
00330   EXPECT_EQ(1, n.failure_count_);
00331 }
00332 
00333 TEST(MessageFilter, emptyFrameIDFailure)
00334 {
00335   tf::TransformListener tf_client;
00336   Notification n(1);
00337   MessageFilter<geometry_msgs::PointStamped> filter(tf_client, "frame1", 1);
00338   filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00339 
00340   geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00341   msg->header.frame_id = "";
00342   filter.add(msg);
00343 
00344   EXPECT_EQ(1, n.failure_count_);
00345 }
00346 
00347 TEST(MessageFilter, removeCallback)
00348 {
00349   // Callback queue in separate thread
00350   ros::CallbackQueue cbqueue;
00351   ros::AsyncSpinner spinner(1, &cbqueue);
00352   ros::NodeHandle threaded_nh;
00353   threaded_nh.setCallbackQueue(&cbqueue);
00354 
00355   // TF filters; no data needs to be published
00356   boost::scoped_ptr<tf::TransformListener> tf_listener;
00357   boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PointStamped> > tf_filter;
00358 
00359   spinner.start();
00360   for (int i = 0; i < 3; ++i) {
00361     tf_listener.reset(new tf::TransformListener());
00362     // Have callback fire at high rate to increase chances of race condition
00363     tf_filter.reset(
00364       new tf::MessageFilter<geometry_msgs::PointStamped>(*tf_listener,
00365                                                          "map", 5, threaded_nh,
00366                                                          ros::Duration(0.000001)));
00367 
00368     // Sleep and reset; sleeping increases chances of race condition
00369     ros::Duration(0.001).sleep();
00370     tf_filter.reset();
00371     tf_listener.reset();
00372   }
00373   spinner.stop();
00374 }
00375 
00376 int main(int argc, char** argv)
00377 {
00378         testing::InitGoogleTest(&argc, argv);
00379 
00380         ros::Time::setNow(ros::Time());
00381         ros::init(argc, argv, "test_message_filter");
00382         ros::NodeHandle nh;
00383 
00384         int ret = RUN_ALL_TESTS();
00385 
00386         return ret;
00387 }


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Wed Jan 3 2018 03:41:25