From c114b9bff8ab06fb37dd6b814170b36464c8d2ec Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 6 Aug 2019 15:26:40 -0700 Subject: [PATCH 1/3] Do not timeout when waiting for transforms Previously, a value of zero was causing timeouts to happen immediately. Signed-off-by: Jacob Perron --- tf2_ros/include/tf2_ros/message_filter.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/tf2_ros/include/tf2_ros/message_filter.h b/tf2_ros/include/tf2_ros/message_filter.h index bdacafabb..22a8c1d83 100644 --- a/tf2_ros/include/tf2_ros/message_filter.h +++ b/tf2_ros/include/tf2_ros/message_filter.h @@ -324,7 +324,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi target_frame, frame_id, tf2::timeFromSec(stamp.seconds()), - tf2::Duration(), + tf2::Duration::max(), std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, next_handle_index_)); try { @@ -349,7 +349,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi target_frame, frame_id, tf2::timeFromSec((stamp + time_tolerance_).seconds()), - tf2::Duration(), + tf2::Duration::max(), std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, next_handle_index_)); try { const auto status = future.wait_for(std::chrono::seconds(0)); From 2a4e1f92f4be52d70615a7aae06f89094cd37be8 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 6 Aug 2019 17:36:44 -0700 Subject: [PATCH 2/3] Make buffer lookup timeout a parameter to MessageFilter The parameter defaults to std::chrono::duration::max(). Signed-off-by: Jacob Perron --- tf2_ros/include/tf2_ros/message_filter.h | 40 +++++++++++++++++------- 1 file changed, 28 insertions(+), 12 deletions(-) diff --git a/tf2_ros/include/tf2_ros/message_filter.h b/tf2_ros/include/tf2_ros/message_filter.h index 22a8c1d83..fee8f1e6e 100644 --- a/tf2_ros/include/tf2_ros/message_filter.h +++ b/tf2_ros/include/tf2_ros/message_filter.h @@ -32,6 +32,7 @@ #ifndef TF2_ROS_MESSAGE_FILTER_H #define TF2_ROS_MESSAGE_FILTER_H +#include #include #include #include @@ -121,12 +122,15 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param node The ros2 node to use for logging and clock operations + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ + template MessageFilter( BufferT & buffer, const std::string & target_frame, uint32_t queue_size, - const rclcpp::Node::SharedPtr & node) + const rclcpp::Node::SharedPtr & node, + std::chrono::duration buffer_timeout = std::chrono::duration::max()) : MessageFilter(buffer, target_frame, queue_size, node->get_node_logging_interface(), - node->get_node_clock_interface()) + node->get_node_clock_interface(), buffer_timeout) { static_assert(std::is_base_of::value, "Buffer type must implement tf2::BufferCoreInterface"); @@ -142,15 +146,19 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param node_logging The logging interface to use for any log messages * \param node_clock The clock interface to use to get the node clock + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ + template MessageFilter( BufferT & buffer, const std::string & target_frame, uint32_t queue_size, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock) + const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock, + std::chrono::duration buffer_timeout = std::chrono::duration::max()) : node_logging_(node_logging), node_clock_(node_clock), buffer_(buffer), - queue_size_(queue_size) + queue_size_(queue_size), + buffer_timeout_(buffer_timeout) { init(); setTargetFrame(target_frame); @@ -164,13 +172,15 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi * \param target_frame The frame this filter should attempt to transform to. To use multiple frames, pass an empty string here and use the setTargetFrames() function. * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param node The ros2 node to use for logging and clock operations + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ - template + template MessageFilter( F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, - const rclcpp::Node::SharedPtr & node) + const rclcpp::Node::SharedPtr & node, + std::chrono::duration buffer_timeout = std::chrono::duration::max()) : MessageFilter(f, buffer, target_frame, queue_size, node->get_node_logging_interface(), - node->get_node_clock_interface()) + node->get_node_clock_interface(), buffer_timeout) { } @@ -183,16 +193,19 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi * \param queue_size The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). * \param node_logging The logging interface to use for any log messages * \param node_clock The clock interface to use to get the node clock + * \param buffer_timeout The timeout duration after requesting transforms from the buffer. */ - template + template MessageFilter( F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size, const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging, - const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock) + const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock, + std::chrono::duration buffer_timeout = std::chrono::duration::max()) : node_logging_(node_logging), node_clock_(node_clock), buffer_(buffer), - queue_size_(queue_size) + queue_size_(queue_size), + buffer_timeout_(buffer_timeout) { init(); setTargetFrame(target_frame); @@ -324,7 +337,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi target_frame, frame_id, tf2::timeFromSec(stamp.seconds()), - tf2::Duration::max(), + buffer_timeout_, std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, next_handle_index_)); try { @@ -349,7 +362,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi target_frame, frame_id, tf2::timeFromSec((stamp + time_tolerance_).seconds()), - tf2::Duration::max(), + buffer_timeout_, std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, next_handle_index_)); try { const auto status = future.wait_for(std::chrono::seconds(0)); @@ -715,6 +728,9 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi message_filters::Connection message_connection_; message_filters::Connection message_connection_failure; + + // Timeout duration when calling the buffer method 'waitForTransform' + tf2::Duration buffer_timeout_; }; } // namespace tf2 From 0c317aa0a342e45e6269b430e73a7ad67a9f3a28 Mon Sep 17 00:00:00 2001 From: Jacob Perron Date: Tue, 6 Aug 2019 17:37:27 -0700 Subject: [PATCH 3/3] Add test for construction/destruction of MessageFilter objects Signed-off-by: Jacob Perron --- tf2_ros/test/message_filter_test.cpp | 66 ++++++++++++++++++++++++++++ 1 file changed, 66 insertions(+) diff --git a/tf2_ros/test/message_filter_test.cpp b/tf2_ros/test/message_filter_test.cpp index fe43306a8..f0b380119 100644 --- a/tf2_ros/test/message_filter_test.cpp +++ b/tf2_ros/test/message_filter_test.cpp @@ -47,6 +47,72 @@ void filter_callback(const geometry_msgs::msg::PointStamped & msg) filter_callback_fired++; } +TEST(tf2_ros_message_filter, construction_and_destruction) +{ + + auto node = rclcpp::Node::make_shared("test_message_filter_node"); + rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); + tf2_ros::Buffer buffer(clock); + + // Node constructor with defaults + { + tf2_ros::MessageFilter filter(buffer, "map", 10, node); + } + + // Node constructor no defaults + { + tf2_ros::MessageFilter filter( + buffer, "map", 10, node, std::chrono::milliseconds(100)); + } + + // Node interface constructor with defaults + { + tf2_ros::MessageFilter filter( + buffer, "map", 10, node->get_node_logging_interface(), node->get_node_clock_interface()); + } + + // Node interface constructor no defaults + { + tf2_ros::MessageFilter filter( + buffer, + "map", + 10, + node->get_node_logging_interface(), + node->get_node_clock_interface(), + std::chrono::seconds(42)); + } + + message_filters::Subscriber sub; + // Filter + node constructor with defaults + { + tf2_ros::MessageFilter filter(sub, buffer, "map", 10, node); + } + + // Filter + node constructor no defaults + { + tf2_ros::MessageFilter filter( + sub, buffer, "map", 10, node, std::chrono::hours(1)); + } + + // Filter + node interface constructor with defaults + { + tf2_ros::MessageFilter filter( + sub, buffer, "map", 10, node->get_node_logging_interface(), node->get_node_clock_interface()); + } + + // Filter + node interface constructor no defaults + { + tf2_ros::MessageFilter filter( + sub, + buffer, + "map", + 10, + node->get_node_logging_interface(), + node->get_node_clock_interface(), + std::chrono::microseconds(0)); + } +} + TEST(tf2_ros_message_filter, multiple_frames_and_time_tolerance) { auto node = rclcpp::Node::make_shared("tf2_ros_message_filter");