$search
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(btTransform(btQuaternion(0,0,0,1), btVector3(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(btTransform(btQuaternion(0,0,0,1), btVector3(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(btTransform(btQuaternion(0,0,0,1), btVector3(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(btTransform(btQuaternion(0,0,0,1), btVector3(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(btTransform(btQuaternion(0,0,0,1), btVector3(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(btTransform(btQuaternion(0,0,0,1), btVector3(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(btTransform(btQuaternion(0,0,0,1), btVector3(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(btTransform(btQuaternion(0,0,0,1), btVector3(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 int main(int argc, char** argv) 00348 { 00349 testing::InitGoogleTest(&argc, argv); 00350 00351 ros::Time::setNow(ros::Time()); 00352 ros::init(argc, argv, "test_message_filter"); 00353 ros::NodeHandle nh; 00354 00355 int ret = RUN_ALL_TESTS(); 00356 00357 return ret; 00358 }