35 #include <geometry_msgs/PointStamped.h>
36 #include <boost/bind.hpp>
37 #include <boost/scoped_ptr.hpp>
41 #include <gtest/gtest.h>
50 , expected_count_(expected_count)
55 void notify(
const geometry_msgs::PointStamped::ConstPtr& message)
77 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
79 msg->header.frame_id =
"frame2";
92 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
94 msg->header.frame_id =
"frame1";
111 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
112 msg->header.stamp = stamp;
113 msg->header.frame_id =
"frame2";
129 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
130 msg->header.stamp = stamp;
131 msg->header.frame_id =
"frame2";
156 for (
int i = 0; i < 20; ++i)
158 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
159 msg->header.stamp = stamp;
160 msg->header.frame_id =
"frame2";
189 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
190 msg->header.stamp = stamp;
191 msg->header.frame_id =
"frame2";
206 std::vector<std::string> target_frames;
207 target_frames.push_back(
"frame1");
208 target_frames.push_back(
"frame2");
215 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
216 msg->header.stamp = stamp;
217 msg->header.frame_id =
"frame3";
249 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
250 msg->header.stamp = stamp;
251 msg->header.frame_id =
"frame2";
258 transform.
stamp_ += offset*1.1;
266 msg->header.stamp = stamp + offset;
286 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
287 msg->header.stamp = stamp;
288 msg->header.frame_id =
"frame2";
291 EXPECT_EQ(0, n.count_);
293 transform.stamp_ = stamp;
299 EXPECT_EQ(0, n.count_);
307 EXPECT_EQ(1, n.count_);
325 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
326 msg->header.stamp = stamp;
327 msg->header.frame_id =
"frame2";
340 geometry_msgs::PointStampedPtr msg(
new geometry_msgs::PointStamped);
341 msg->header.frame_id =
"";
356 boost::scoped_ptr<tf::TransformListener> tf_listener;
357 boost::scoped_ptr<tf::MessageFilter<geometry_msgs::PointStamped> > tf_filter;
360 for (
int i = 0; i < 3; ++i) {
365 "map", 5, threaded_nh,
376 int main(
int argc,
char** argv)
378 testing::InitGoogleTest(&argc, argv);
381 ros::init(argc, argv,
"test_message_filter");
384 int ret = RUN_ALL_TESTS();