00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
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_);
00224
00225
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_);
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_);
00255
00256
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_);
00265
00266 msg->header.stamp = stamp + offset;
00267 filter.add(msg);
00268
00269 EXPECT_EQ(1, n.count_);
00270 }
00271
00272
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
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 }