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 <tf2_ros/message_filter.h>
00033 #include <tf2/buffer_core.h>
00034 #include <geometry_msgs/PointStamped.h>
00035 #include <boost/bind.hpp>
00036 #include <boost/scoped_ptr.hpp>
00037
00038 #include "ros/ros.h"
00039 #include "ros/callback_queue.h"
00040
00041 #include <gtest/gtest.h>
00042
00043 using namespace tf2;
00044 using namespace tf2_ros;
00045
00046 class Notification
00047 {
00048 public:
00049 Notification(int expected_count) :
00050 count_(0), expected_count_(expected_count), failure_count_(0)
00051 {
00052 }
00053
00054 void notify(const geometry_msgs::PointStamped::ConstPtr& message)
00055 {
00056 ++count_;
00057 }
00058
00059 void failure(const geometry_msgs::PointStamped::ConstPtr& message, FilterFailureReason reason)
00060 {
00061 ++failure_count_;
00062 }
00063
00064 int count_;
00065 int expected_count_;
00066 int failure_count_;
00067 };
00068
00069 TEST(MessageFilter, noTransforms)
00070 {
00071 BufferCore bc;
00072 Notification n(1);
00073 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00074 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00075
00076 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00077 msg->header.stamp = ros::Time(1);
00078 msg->header.frame_id = "frame2";
00079 filter.add(msg);
00080
00081 EXPECT_EQ(0, n.count_);
00082 }
00083
00084 TEST(MessageFilter, noTransformsSameFrame)
00085 {
00086 BufferCore bc;
00087 Notification n(1);
00088 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00089 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00090
00091 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00092 msg->header.stamp = ros::Time(1);
00093 msg->header.frame_id = "frame1";
00094 filter.add(msg);
00095
00096 EXPECT_EQ(1, n.count_);
00097 }
00098
00099 geometry_msgs::TransformStamped createTransform(btQuaternion q, btVector3 v, ros::Time stamp, const std::string& frame1, const std::string& frame2)
00100 {
00101 geometry_msgs::TransformStamped t;
00102 t.header.frame_id = frame1;
00103 t.child_frame_id = frame2;
00104 t.header.stamp = stamp;
00105 t.transform.translation.x = v.x();
00106 t.transform.translation.y = v.y();
00107 t.transform.translation.z = v.z();
00108 t.transform.rotation.x = q.x();
00109 t.transform.rotation.y = q.y();
00110 t.transform.rotation.z = q.z();
00111 t.transform.rotation.w = q.w();
00112 return t;
00113 }
00114
00115 TEST(MessageFilter, preexistingTransforms)
00116 {
00117 BufferCore bc;
00118 Notification n(1);
00119 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00120 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00121
00122 ros::Time stamp(1);
00123 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp, "frame1", "frame2"), "me");
00124
00125 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00126 msg->header.stamp = stamp;
00127 msg->header.frame_id = "frame2";
00128
00129 filter.add(msg);
00130
00131 EXPECT_EQ(1, n.count_);
00132 }
00133
00134 TEST(MessageFilter, postTransforms)
00135 {
00136 BufferCore bc;
00137 Notification n(1);
00138 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00139 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00140
00141 ros::Time stamp(1);
00142
00143 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00144 msg->header.stamp = stamp;
00145 msg->header.frame_id = "frame2";
00146
00147 filter.add(msg);
00148
00149 EXPECT_EQ(0, n.count_);
00150
00151 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp, "frame1", "frame2"), "me");
00152
00153 EXPECT_EQ(1, n.count_);
00154 }
00155
00156 TEST(MessageFilter, queueSize)
00157 {
00158 BufferCore bc;
00159 Notification n(10);
00160 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 10, 0);
00161 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00162 filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00163
00164 ros::Time stamp(1);
00165
00166 for (int i = 0; i < 20; ++i)
00167 {
00168 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00169 msg->header.stamp = stamp;
00170 msg->header.frame_id = "frame2";
00171
00172 filter.add(msg);
00173 }
00174
00175 EXPECT_EQ(0, n.count_);
00176 EXPECT_EQ(10, n.failure_count_);
00177
00178 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp, "frame1", "frame2"), "me");
00179
00180 EXPECT_EQ(10, n.count_);
00181 }
00182
00183 TEST(MessageFilter, setTargetFrame)
00184 {
00185 BufferCore bc;
00186 Notification n(1);
00187 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00188 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00189 filter.setTargetFrame("frame1000");
00190
00191 ros::Time stamp(1);
00192 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp, "frame1000", "frame2"), "me");
00193
00194 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00195 msg->header.stamp = stamp;
00196 msg->header.frame_id = "frame2";
00197
00198 filter.add(msg);
00199
00200 EXPECT_EQ(1, n.count_);
00201 }
00202
00203
00204 TEST(MessageFilter, multipleTargetFrames)
00205 {
00206 BufferCore bc;
00207 Notification n(1);
00208 MessageFilter<geometry_msgs::PointStamped> filter(bc, "", 1, 0);
00209 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00210
00211 std::vector<std::string> target_frames;
00212 target_frames.push_back("frame1");
00213 target_frames.push_back("frame2");
00214 filter.setTargetFrames(target_frames);
00215
00216 ros::Time stamp(1);
00217 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp, "frame1", "frame3"), "me");
00218
00219 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00220 msg->header.stamp = stamp;
00221 msg->header.frame_id = "frame3";
00222 filter.add(msg);
00223
00224 EXPECT_EQ(0, n.count_);
00225
00226
00227
00228 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp, "frame1", "frame2"), "me");
00229
00230 EXPECT_EQ(1, n.count_);
00231 }
00232
00233 TEST(MessageFilter, tolerance)
00234 {
00235 ros::Duration offset(0.2);
00236 BufferCore bc;
00237 Notification n(1);
00238 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00239 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00240 filter.setTolerance(offset);
00241
00242 ros::Time stamp(1);
00243 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp, "frame1", "frame2"), "me");
00244
00245 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00246 msg->header.stamp = stamp;
00247 msg->header.frame_id = "frame2";
00248 filter.add(msg);
00249
00250 EXPECT_EQ(0, n.count_);
00251
00252 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp + (offset * 1.1), "frame1", "frame2"), "me");
00253
00254 EXPECT_EQ(1, n.count_);
00255
00256 msg->header.stamp = stamp + offset;
00257 filter.add(msg);
00258
00259 EXPECT_EQ(1, n.count_);
00260 }
00261
00262 TEST(MessageFilter, outTheBackFailure)
00263 {
00264 BufferCore bc;
00265 Notification n(1);
00266 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00267 filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00268
00269 ros::Time stamp(1);
00270 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp, "frame1", "frame2"), "me");
00271 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp + ros::Duration(10000), "frame1", "frame2"), "me");
00272
00273 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00274 msg->header.stamp = stamp;
00275 msg->header.frame_id = "frame2";
00276 filter.add(msg);
00277
00278 EXPECT_EQ(1, n.failure_count_);
00279 }
00280
00281 TEST(MessageFilter, outTheBackFailure2)
00282 {
00283 BufferCore bc;
00284 Notification n(1);
00285 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00286 filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00287
00288 ros::Time stamp(1);
00289
00290 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00291 msg->header.stamp = stamp;
00292 msg->header.frame_id = "frame2";
00293 filter.add(msg);
00294
00295 EXPECT_EQ(0, n.count_);
00296 EXPECT_EQ(0, n.failure_count_);
00297
00298 bc.setTransform(createTransform(btQuaternion(0,0,0,1), btVector3(1,2,3), stamp + ros::Duration(10000), "frame1", "frame2"), "me");
00299
00300 EXPECT_EQ(1, n.failure_count_);
00301 }
00302
00303 TEST(MessageFilter, emptyFrameIDFailure)
00304 {
00305 BufferCore bc;
00306 Notification n(1);
00307 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, 0);
00308 filter.registerFailureCallback(boost::bind(&Notification::failure, &n, _1, _2));
00309
00310 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00311 msg->header.frame_id = "";
00312 filter.add(msg);
00313
00314 EXPECT_EQ(1, n.failure_count_);
00315 }
00316
00317 TEST(MessageFilter, callbackQueue)
00318 {
00319 BufferCore bc;
00320 Notification n(1);
00321 ros::CallbackQueue queue;
00322 MessageFilter<geometry_msgs::PointStamped> filter(bc, "frame1", 1, &queue);
00323 filter.registerCallback(boost::bind(&Notification::notify, &n, _1));
00324
00325 geometry_msgs::PointStampedPtr msg(new geometry_msgs::PointStamped);
00326 msg->header.stamp = ros::Time(1);
00327 msg->header.frame_id = "frame1";
00328 filter.add(msg);
00329
00330 EXPECT_EQ(0, n.count_);
00331
00332 queue.callAvailable();
00333
00334 EXPECT_EQ(1, n.count_);
00335 }
00336
00337
00338 int main(int argc, char** argv)
00339 {
00340 testing::InitGoogleTest(&argc, argv);
00341
00342 int ret = RUN_ALL_TESTS();
00343
00344 return ret;
00345 }