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