Skip to content

Use node interface pointers in the MessageFilter class #96

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

Merged
tfoote merged 2 commits into from
Mar 13, 2019
Merged

Use node interface pointers in the MessageFilter class #96

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
027cc6e
Use node inteface pointers in the MessageFilter class
Mar 6, 2019
832e45a
Update comments
Mar 6, 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
67 changes: 53 additions & 14 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 @@ -119,17 +119,35 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
* \param bc The tf2::BufferCore this filter should use
* \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 whose callback queue we should add callbacks to
* \param node The ros2 node to use for logging and clock operations
*/
MessageFilter(
tf2::BufferCore & bc, const std::string & target_frame, uint32_t queue_size,
const rclcpp::Node::SharedPtr & node)
: MessageFilter(bc, target_frame, queue_size, node->get_node_logging_interface(),
node->get_node_clock_interface())
{
}

/**
* \brief Constructor
*
* \param bc The tf2::BufferCore this filter should use
* \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_logging The logging interface to use for any log messages
* \param node_clock The clock interface to use to get the node clock
*/
MessageFilter(
tf2::BufferCore & bc, 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)
: bc_(bc),
queue_size_(queue_size),
node_(node)
node_logging_(node_logging),
node_clock_(node_clock)
{
init();

setTargetFrame(target_frame);
}

Expand All @@ -140,24 +158,42 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
* \param bc The tf2::BufferCore this filter should use
* \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 whose callback queue we should add callbacks to
* \param node The ros2 node to use for logging and clock operations
*/
template<class F>
MessageFilter(
F & f, tf2::BufferCore & bc, const std::string & target_frame, uint32_t queue_size,
const rclcpp::Node::SharedPtr & node)
: MessageFilter(f, bc, target_frame, queue_size, node->get_node_logging_interface(),
node->get_node_clock_interface())
{
}

/**
* \brief Constructor
*
* \param f The filter to connect this filter's input to. Often will be a message_filters::Subscriber.
* \param bc The tf2::BufferCore this filter should use
* \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_logging The logging interface to use for any log messages
* \param node_clock The clock interface to use to get the node clock
*/
template<class F>
MessageFilter(
F & f, tf2::BufferCore & bc, 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)
: bc_(bc),
queue_size_(queue_size),
node_(node)
node_logging_(node_logging),
node_clock_(node_clock)
{
init();

setTargetFrame(target_frame);

connectInput(f);
}


/**
* \brief Connect this filter's input to another filter's output. If this filter is already connected, disconnects first.
*/
Expand Down Expand Up @@ -371,7 +407,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
std::string>);

(*header)["callerid"] = "unknown";
Time t = node_->now();
Time t = node_clock_->get_clock()->now();
add(MEvent(message, header, t));
}

Expand Down Expand Up @@ -510,10 +546,10 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
void checkFailures()
{
if (!next_failure_warning_.nanoseconds()) {
next_failure_warning_ = node_->now() + rclcpp::Duration(15, 0);
next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(15, 0);
}

if (node_->now() >= next_failure_warning_) {
if (node_clock_->get_clock()->now() >= next_failure_warning_) {
if (incoming_message_count_ - message_count_ == 0) {
return;
}
Expand All @@ -524,7 +560,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
TF2_ROS_MESSAGEFILTER_WARN(
"Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct * 100,
"tf2_ros_message_filter");
next_failure_warning_ = node_->now() + rclcpp::Duration(60, 0);
next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(60, 0);

if (static_cast<double>(failed_out_the_back_count_) / static_cast<double>(dropped_message_count_) > 0.5) {
TF2_ROS_MESSAGEFILTER_WARN(
Expand Down Expand Up @@ -605,7 +641,7 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
const MConstPtr & message = evt.getMessage();
std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
rclcpp::Time stamp = mt::TimeStamp<M>::value(*message);
RCLCPP_INFO(node_->get_logger(), "[%s] Drop message: frame '%s' at time %.3f for reason(%d)",
RCLCPP_INFO(node_logging_->get_logger(), "[%s] Drop message: frame '%s' at time %.3f for reason(%d)",
__func__, frame_id.c_str(), stamp.seconds(), reason);
}

Expand All @@ -620,7 +656,10 @@ class MessageFilter : public MessageFilterBase, public message_filters::SimpleFi
return in;
}

const rclcpp::Node::SharedPtr node_;
///< The node logging interface to use for any log messages
const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
///< The node clock interface to use to get the clock to use
const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
///< The Transformer used to determine if transformation data is available
tf2::BufferCore & bc_;
///< The frames we need to be able to transform to before a message is ready
Expand Down