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
00034 #include <pcl/point_types.h>
00035 #include <pcl/point_cloud.h>
00036
00037 #include <tf/message_filter.h>
00038 #include <tf/transform_listener.h>
00039 #include <tf/transform_broadcaster.h>
00040
00041 #include <boost/bind.hpp>
00042 #include <boost/scoped_ptr.hpp>
00043
00044 #include <pcl_ros/point_cloud.h>
00045 #include <pcl_conversions/pcl_conversions.h>
00046
00047 #include <ros/ros.h>
00048
00049 #include <gtest/gtest.h>
00050
00051 using namespace tf;
00052
00053
00054
00055 typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PCDType;
00056
00057
00061 void setStamp(ros::Time &stamp, pcl::uint64_t &pcl_stamp)
00062 {
00063
00064 static const uint32_t mult = 1e6;
00065 stamp.nsec /= mult;
00066 stamp.nsec *= mult;
00067
00068 pcl_conversions::toPCL(stamp, pcl_stamp);
00069
00070
00071 {
00072 ros::Time t;
00073 pcl_conversions::fromPCL(pcl_stamp, t);
00074 ROS_ASSERT_MSG(t==stamp, "%d/%d vs %d/%d", t.sec, t.nsec, stamp.sec, stamp.nsec);
00075 }
00076 }
00077
00078 class Notification
00079 {
00080 public:
00081 Notification(int expected_count)
00082 : count_(0)
00083 , expected_count_(expected_count)
00084 , failure_count_(0)
00085 {
00086 }
00087
00088 void notify(const PCDType::ConstPtr& message)
00089 {
00090 ++count_;
00091 }
00092
00093 void failure(const PCDType::ConstPtr& message, FilterFailureReason reason)
00094 {
00095 ++failure_count_;
00096 }
00097
00098 int count_;
00099 int expected_count_;
00100 int failure_count_;
00101 };
00102
00103 TEST(MessageFilter, noTransforms)
00104 {
00105 tf::TransformListener tf_client;
00106 Notification n(1);
00107 MessageFilter<PCDType> filter(tf_client, "frame1", 1);
00108 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00109
00110 PCDType::Ptr msg(new PCDType);
00111 ros::Time stamp = ros::Time::now();
00112 setStamp(stamp, msg->header.stamp);
00113 msg->header.frame_id = "frame2";
00114 filter.add(msg);
00115
00116 EXPECT_EQ(0, n.count_);
00117 }
00118
00119 TEST(MessageFilter, noTransformsSameFrame)
00120 {
00121 tf::TransformListener tf_client;
00122 Notification n(1);
00123 MessageFilter<PCDType> filter(tf_client, "frame1", 1);
00124 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00125
00126 PCDType::Ptr msg(new PCDType);
00127 ros::Time stamp = ros::Time::now();
00128 setStamp(stamp, msg->header.stamp);
00129 msg->header.frame_id = "frame1";
00130 filter.add(msg);
00131
00132 EXPECT_EQ(1, n.count_);
00133 }
00134
00135 TEST(MessageFilter, preexistingTransforms)
00136 {
00137 tf::TransformListener tf_client;
00138 Notification n(1);
00139 MessageFilter<PCDType> filter(tf_client, "frame1", 1);
00140 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00141
00142 PCDType::Ptr msg(new PCDType);
00143
00144 ros::Time stamp = ros::Time::now();
00145 setStamp(stamp, msg->header.stamp);
00146
00147 tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00148 tf_client.setTransform(transform);
00149
00150 msg->header.frame_id = "frame2";
00151 filter.add(msg);
00152
00153 EXPECT_EQ(1, n.count_);
00154 }
00155
00156 TEST(MessageFilter, postTransforms)
00157 {
00158 tf::TransformListener tf_client;
00159 Notification n(1);
00160 MessageFilter<PCDType> filter(tf_client, "frame1", 1);
00161 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00162
00163 ros::Time stamp = ros::Time::now();
00164 PCDType::Ptr msg(new PCDType);
00165 setStamp(stamp, msg->header.stamp);
00166 msg->header.frame_id = "frame2";
00167
00168 filter.add(msg);
00169
00170 EXPECT_EQ(0, n.count_);
00171
00172 tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00173 tf_client.setTransform(transform);
00174
00175 ros::WallDuration(0.1).sleep();
00176 ros::spinOnce();
00177
00178 EXPECT_EQ(1, n.count_);
00179 }
00180
00181 TEST(MessageFilter, queueSize)
00182 {
00183 tf::TransformListener tf_client;
00184 Notification n(10);
00185 MessageFilter<PCDType> filter(tf_client, "frame1", 10);
00186 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00187 filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00188
00189 ros::Time stamp = ros::Time::now();
00190 pcl::uint64_t pcl_stamp;
00191 setStamp(stamp, pcl_stamp);
00192
00193 for (int i = 0; i < 20; ++i)
00194 {
00195 PCDType::Ptr msg(new PCDType);
00196 msg->header.stamp = pcl_stamp;
00197 msg->header.frame_id = "frame2";
00198
00199 filter.add(msg);
00200 }
00201
00202 EXPECT_EQ(0, n.count_);
00203 EXPECT_EQ(10, n.failure_count_);
00204
00205 tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00206 tf_client.setTransform(transform);
00207
00208 ros::WallDuration(0.1).sleep();
00209 ros::spinOnce();
00210
00211 EXPECT_EQ(10, n.count_);
00212 }
00213
00214 TEST(MessageFilter, setTargetFrame)
00215 {
00216 tf::TransformListener tf_client;
00217 Notification n(1);
00218 MessageFilter<PCDType> filter(tf_client, "frame1", 1);
00219 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00220 filter.setTargetFrame("frame1000");
00221
00222 ros::Time stamp = ros::Time::now();
00223 PCDType::Ptr msg(new PCDType);
00224 setStamp(stamp, msg->header.stamp);
00225 msg->header.frame_id = "frame2";
00226
00227 tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1000", "frame2");
00228 tf_client.setTransform(transform);
00229
00230 filter.add(msg);
00231
00232
00233 EXPECT_EQ(1, n.count_);
00234 }
00235
00236 TEST(MessageFilter, multipleTargetFrames)
00237 {
00238 tf::TransformListener tf_client;
00239 Notification n(1);
00240 MessageFilter<PCDType> filter(tf_client, "", 1);
00241 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00242
00243 std::vector<std::string> target_frames;
00244 target_frames.push_back("frame1");
00245 target_frames.push_back("frame2");
00246 filter.setTargetFrames(target_frames);
00247
00248 ros::Time stamp = ros::Time::now();
00249 PCDType::Ptr msg(new PCDType);
00250 setStamp(stamp, msg->header.stamp);
00251
00252 tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame3");
00253 tf_client.setTransform(transform);
00254
00255 msg->header.frame_id = "frame3";
00256 filter.add(msg);
00257
00258 ros::WallDuration(0.1).sleep();
00259 ros::spinOnce();
00260
00261 EXPECT_EQ(0, n.count_);
00262
00263
00264
00265 transform.child_frame_id_ = "frame2";
00266 tf_client.setTransform(transform);
00267
00268 ros::WallDuration(0.1).sleep();
00269 ros::spinOnce();
00270
00271 EXPECT_EQ(1, n.count_);
00272 }
00273
00274 TEST(MessageFilter, tolerance)
00275 {
00276 ros::Duration offset(0.2);
00277 tf::TransformListener tf_client;
00278 Notification n(1);
00279 MessageFilter<PCDType> filter(tf_client, "frame1", 1);
00280 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00281 filter.setTolerance(offset);
00282
00283 ros::Time stamp = ros::Time::now();
00284 pcl::uint64_t pcl_stamp;
00285 setStamp(stamp, pcl_stamp);
00286 tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00287 tf_client.setTransform(transform);
00288
00289 PCDType::Ptr msg(new PCDType);
00290 msg->header.frame_id = "frame2";
00291 msg->header.stamp = pcl_stamp;
00292 filter.add(msg);
00293
00294 EXPECT_EQ(0, n.count_);
00295
00296
00297
00298 transform.stamp_ += offset*1.1;
00299 tf_client.setTransform(transform);
00300
00301 ros::WallDuration(0.1).sleep();
00302 ros::spinOnce();
00303
00304 EXPECT_EQ(1, n.count_);
00305
00306 stamp += offset;
00307 setStamp(stamp, pcl_stamp);
00308 msg->header.stamp = pcl_stamp;
00309
00310 filter.add(msg);
00311
00312 EXPECT_EQ(1, n.count_);
00313 }
00314
00315 TEST(MessageFilter, outTheBackFailure)
00316 {
00317 tf::TransformListener tf_client;
00318 Notification n(1);
00319 MessageFilter<PCDType> filter(tf_client, "frame1", 1);
00320 filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00321
00322 ros::Time stamp = ros::Time::now();
00323 PCDType::Ptr msg(new PCDType);
00324 setStamp(stamp, msg->header.stamp);
00325
00326 tf::StampedTransform transform(tf::Transform(tf::Quaternion(0,0,0,1), tf::Vector3(1,2,3)), stamp, "frame1", "frame2");
00327 tf_client.setTransform(transform);
00328
00329 transform.stamp_ = stamp + ros::Duration(10000);
00330 tf_client.setTransform(transform);
00331
00332 msg->header.frame_id = "frame2";
00333 filter.add(msg);
00334
00335 EXPECT_EQ(1, n.failure_count_);
00336 }
00337
00338 TEST(MessageFilter, emptyFrameIDFailure)
00339 {
00340 tf::TransformListener tf_client;
00341 Notification n(1);
00342 MessageFilter<PCDType> filter(tf_client, "frame1", 1);
00343 filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00344
00345 PCDType::Ptr msg(new PCDType);
00346 msg->header.frame_id = "";
00347 filter.add(msg);
00348
00349 EXPECT_EQ(1, n.failure_count_);
00350 }
00351
00352 TEST(MessageFilter, removeCallback)
00353 {
00354
00355 ros::CallbackQueue cbqueue;
00356 ros::AsyncSpinner spinner(1, &cbqueue);
00357 ros::NodeHandle threaded_nh;
00358 threaded_nh.setCallbackQueue(&cbqueue);
00359
00360
00361 boost::scoped_ptr<tf::TransformListener> tf_listener;
00362 boost::scoped_ptr<tf::MessageFilter<PCDType> > tf_filter;
00363
00364 spinner.start();
00365 for (int i = 0; i < 3; ++i) {
00366 tf_listener.reset(new tf::TransformListener());
00367
00368 tf_filter.reset(
00369 new tf::MessageFilter<PCDType>(*tf_listener,
00370 "map", 5, threaded_nh,
00371 ros::Duration(0.000001)));
00372
00373
00374 ros::Duration(0.001).sleep();
00375 tf_filter.reset();
00376 tf_listener.reset();
00377 }
00378 spinner.stop();
00379 }
00380
00381 int main(int argc, char** argv)
00382 {
00383 testing::InitGoogleTest(&argc, argv);
00384
00385 ros::Time::setNow(ros::Time());
00386 ros::init(argc, argv, "test_message_filter");
00387 ros::NodeHandle nh;
00388
00389 int ret = RUN_ALL_TESTS();
00390
00391 return ret;
00392 }