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";
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();
TEST(MessageFilter, noTransforms)
int main(int argc, char **argv)
Follows the patterns set by the message_filters package to implement a filter which only passes messa...
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
void add(const MEvent &evt)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Notification(int expected_count)
static void setNow(const Time &new_now)
void setTargetFrames(const std::vector< std::string > &target_frames)
Set the frames you need to be able to transform to before getting a message callback.
void setCallbackQueue(CallbackQueueInterface *queue)
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
Register a callback to be called when a message is about to be dropped.
void notify(const geometry_msgs::PointStamped::ConstPtr &message)
void setTargetFrame(const std::string &target_frame)
Set the frame you need to be able to transform to before getting a message callback.
void failure(const geometry_msgs::PointStamped::ConstPtr &message, FilterFailureReason reason)
ROSCPP_DECL void spinOnce()
void setTolerance(const ros::Duration &tolerance)
Set the required tolerance for the notifier to return true.
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Connection registerCallback(const C &callback)