message_filter_test.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2014, Open Source Robotics Foundation
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <geometry_msgs/PointStamped.h>
31 #include <geometry_msgs/TransformStamped.h>
33 #include <ros/ros.h>
34 #include <tf2_ros/buffer.h>
35 #include <tf2_ros/message_filter.h>
38 
39 #include <gtest/gtest.h>
40 #include <thread>
41 #include <chrono>
42 
43 
45 {
46  ros::spinOnce();
47  for (uint8_t i = 0; i < 10; ++i)
48  {
49  std::this_thread::sleep_for(std::chrono::microseconds(100));
50  ros::spinOnce();
51  }
52 }
53 
54 bool filter_callback_fired = false;
55 void filter_callback(const geometry_msgs::PointStamped& msg)
56 {
57  filter_callback_fired = true;
58 }
59 
60 TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance)
61 {
62  ros::NodeHandle nh;
64  sub.subscribe(nh, "point", 10);
65 
66  tf2_ros::Buffer buffer;
67  tf2_ros::TransformListener tfl(buffer);
68  tf2_ros::MessageFilter<geometry_msgs::PointStamped> filter(buffer, "map", 10, nh);
69  filter.connectInput(sub);
71  // Register multiple target frames
72  std::vector<std::string> frames;
73  frames.push_back("odom");
74  frames.push_back("map");
75  filter.setTargetFrames(frames);
76  // Set a non-zero time tolerance
77  filter.setTolerance(ros::Duration(1, 0));
78 
79  // Publish static transforms so the frame transformations will always be valid
81  geometry_msgs::TransformStamped map_to_odom;
82  map_to_odom.header.stamp = ros::Time(0, 0);
83  map_to_odom.header.frame_id = "map";
84  map_to_odom.child_frame_id = "odom";
85  map_to_odom.transform.translation.x = 0.0;
86  map_to_odom.transform.translation.y = 0.0;
87  map_to_odom.transform.translation.z = 0.0;
88  map_to_odom.transform.rotation.x = 0.0;
89  map_to_odom.transform.rotation.y = 0.0;
90  map_to_odom.transform.rotation.z = 0.0;
91  map_to_odom.transform.rotation.w = 1.0;
92  tfb.sendTransform(map_to_odom);
93 
94  geometry_msgs::TransformStamped odom_to_base;
95  odom_to_base.header.stamp = ros::Time(0, 0);
96  odom_to_base.header.frame_id = "odom";
97  odom_to_base.child_frame_id = "base";
98  odom_to_base.transform.translation.x = 0.0;
99  odom_to_base.transform.translation.y = 0.0;
100  odom_to_base.transform.translation.z = 0.0;
101  odom_to_base.transform.rotation.x = 0.0;
102  odom_to_base.transform.rotation.y = 0.0;
103  odom_to_base.transform.rotation.z = 0.0;
104  odom_to_base.transform.rotation.w = 1.0;
105  tfb.sendTransform(odom_to_base);
106 
107  // Publish a Point message in the "base" frame
108  ros::Publisher pub = nh.advertise<geometry_msgs::PointStamped>("point", 10);
109  geometry_msgs::PointStamped point;
110  point.header.stamp = ros::Time::now();
111  point.header.frame_id = "base";
112  pub.publish(point);
113 
114  // make sure it arrives
116 
117  // The filter callback should have been fired because all required transforms are available
118  ASSERT_TRUE(filter_callback_fired);
119 }
120 
121 int main(int argc, char **argv){
122  testing::InitGoogleTest(&argc, argv);
123  ros::init(argc, argv, "tf2_ros_message_filter");
124  return RUN_ALL_TESTS();
125 }
tf2_ros::StaticTransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
Send a TransformStamped message The stamped data structure includes frame_id, and time,...
Definition: static_transform_broadcaster.h:57
ros::Publisher
tf2_ros::MessageFilter
Follows the patterns set by the message_filters package to implement a filter which only passes messa...
Definition: message_filter.h:104
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros.h
ros::spinOnce
ROSCPP_DECL void spinOnce()
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
message_filters::Subscriber
tf2_ros::TransformListener
This class provides an easy way to request and receive coordinate frame transform information.
Definition: transform_listener.h:48
static_transform_broadcaster.h
tf2_ros::StaticTransformBroadcaster
This class provides an easy way to publish coordinate frame transform information....
Definition: static_transform_broadcaster.h:50
filter_callback_fired
bool filter_callback_fired
Definition: message_filter_test.cpp:54
subscriber.h
filter_callback
void filter_callback(const geometry_msgs::PointStamped &msg)
Definition: message_filter_test.cpp:55
tf2_ros::MessageFilter::setTolerance
void setTolerance(const ros::Duration &tolerance)
Set the required tolerance for the notifier to return true.
Definition: message_filter.h:268
transform_listener.h
tf2_ros::Buffer
Standard implementation of the tf2_ros::BufferInterface abstract data type.
Definition: buffer.h:51
message_filters::SimpleFilter< M >::registerCallback
Connection registerCallback(const boost::function< void(P)> &callback)
buffer.h
tf2_ros::MessageFilter::setTargetFrames
void setTargetFrames(const V_string &target_frames)
Set the frames you need to be able to transform to before getting a message callback.
Definition: message_filter.h:241
tf2_ros::MessageFilter::connectInput
void connectInput(F &f)
Connect this filter's input to another filter's output. If this filter is already connected,...
Definition: message_filter.h:206
message_filters::Subscriber::subscribe
void subscribe()
ros::Time
message_filter.h
ros::Duration
spin_for_a_second
void spin_for_a_second()
Definition: message_filter_test.cpp:44
TEST
TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance)
Definition: message_filter_test.cpp:60
main
int main(int argc, char **argv)
Definition: message_filter_test.cpp:121
ros::NodeHandle
ros::Time::now
static Time now()


tf2_ros
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Sep 4 2020 03:15:40