Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Do not timeout when waiting for transforms #146

Merged
merged 3 commits into from
Aug 7, 2019
Merged

Do not timeout when waiting for transforms #146

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c114b9b
Do not timeout when waiting for transforms
jacobperron Aug 6, 2019
2a4e1f9
Make buffer lookup timeout a parameter to MessageFilter
jacobperron Aug 7, 2019
0c317aa
Add test for construction/destruction of MessageFilter objects
jacobperron Aug 7, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 28 additions & 12 deletions tf2_ros/include/tf2_ros/message_filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@
#ifndef TF2_ROS_MESSAGE_FILTER_H
#define TF2_ROS_MESSAGE_FILTER_H

#include <chrono>
#include <list>
#include <memory>
#include <string>
Expand Down Expand Up @@ -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<typename TimeRepT = int64_t, typename TimeT = std::nano>
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<TimeRepT, TimeT> buffer_timeout = std::chrono::duration<TimeRepT, TimeT>::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<tf2::BufferCoreInterface, BufferT>::value,
"Buffer type must implement tf2::BufferCoreInterface");
Expand All @@ -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<typename TimeRepT = int64_t, typename TimeT = std::nano>
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<TimeRepT, TimeT> buffer_timeout = std::chrono::duration<TimeRepT, TimeT>::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);
Expand All @@ -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<class F>
template<class F, typename TimeRepT = int64_t, typename TimeT = std::nano>
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<TimeRepT, TimeT> buffer_timeout = std::chrono::duration<TimeRepT, TimeT>::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)
{
}

Expand All @@ -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<class F>
template<class F, typename TimeRepT = int64_t, typename TimeT = std::nano>
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<TimeRepT, TimeT> buffer_timeout = std::chrono::duration<TimeRepT, TimeT>::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);
Expand Down Expand Up @@ -324,7 +337,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
target_frame,
frame_id,
tf2::timeFromSec(stamp.seconds()),
tf2::Duration(),
buffer_timeout_,
std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, next_handle_index_));

try {
Expand All @@ -349,7 +362,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
target_frame,
frame_id,
tf2::timeFromSec((stamp + time_tolerance_).seconds()),
tf2::Duration(),
buffer_timeout_,
std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, next_handle_index_));
try {
const auto status = future.wait_for(std::chrono::seconds(0));
Expand Down Expand Up @@ -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
Expand Down
66 changes: 66 additions & 0 deletions tf2_ros/test/message_filter_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<rclcpp::Clock>(RCL_SYSTEM_TIME);
tf2_ros::Buffer buffer(clock);

// Node constructor with defaults
{
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(buffer, "map", 10, node);
}

// Node constructor no defaults
{
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(
buffer, "map", 10, node, std::chrono::milliseconds(100));
}

// Node interface constructor with defaults
{
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(
buffer, "map", 10, node->get_node_logging_interface(), node->get_node_clock_interface());
}

// Node interface constructor no defaults
{
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(
buffer,
"map",
10,
node->get_node_logging_interface(),
node->get_node_clock_interface(),
std::chrono::seconds(42));
}

message_filters::Subscriber<geometry_msgs::msg::PointStamped> sub;
// Filter + node constructor with defaults
{
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(sub, buffer, "map", 10, node);
}

// Filter + node constructor no defaults
{
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(
sub, buffer, "map", 10, node, std::chrono::hours(1));
}

// Filter + node interface constructor with defaults
{
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> filter(
sub, buffer, "map", 10, node->get_node_logging_interface(), node->get_node_clock_interface());
}

// Filter + node interface constructor no defaults
{
tf2_ros::MessageFilter<geometry_msgs::msg::PointStamped> 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");
Expand Down