tf2_ros  master
This package contains the ROS bindings for the tf2 library, for both Python and C++.
message_filter.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010, Willow Garage, Inc.
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 
32 #ifndef TF2_ROS_MESSAGE_FILTER_H
33 #define TF2_ROS_MESSAGE_FILTER_H
34 
35 #include <chrono>
36 #include <list>
37 #include <memory>
38 #include <string>
39 #include <vector>
40 
41 #include <message_filters/connection.h>
42 #include <message_filters/message_traits.h>
43 #include <message_filters/simple_filter.h>
44 #include <rclcpp/rclcpp.hpp>
45 #include <tf2/buffer_core_interface.h>
46 #include <tf2/time.h>
47 
48 #include <tf2_ros/buffer.h>
49 
50 #define TF2_ROS_MESSAGEFILTER_DEBUG(fmt, ...) \
51  RCUTILS_LOG_DEBUG_NAMED("tf2_ros_message_filter", \
52  std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), \
53  getTargetFramesString().c_str(), __VA_ARGS__)
54 
55 #define TF2_ROS_MESSAGEFILTER_WARN(fmt, ...) \
56  RCUTILS_LOG_WARN_NAMED("tf2_ros_message_filter", \
57  std::string(std::string("MessageFilter [target=%s]: ") + std::string(fmt)).c_str(), \
58  getTargetFramesString().c_str(), __VA_ARGS__)
59 
60 namespace tf2_ros
61 {
62 
63 namespace filter_failure_reasons
64 {
66 {
67  // NOTE when adding new values, do not explicitly assign a number. See FilterFailureReasonCount
68 
77 };
78 
79 }
80 
81 static std::string get_filter_failure_reason_string(filter_failure_reasons::FilterFailureReason reason) {
82  switch (reason) {
84  return "Unknown";
86  return "OutTheBack";
88  return "EmptyFrameID";
89  default:
90  return "Invalid Failure Reason";
91  }
92 }
93 
95 
97 {
98 public:
100 
101  virtual ~MessageFilterBase() {}
102  virtual void clear() = 0;
103  virtual void setTargetFrame(const std::string & target_frame) = 0;
104  virtual void setTargetFrames(const V_string & target_frames) = 0;
105  virtual void setTolerance(const rclcpp::Duration & tolerance) = 0;
106 };
107 
124 template<class M, class BufferT = tf2_ros::Buffer>
125 class MessageFilter : public MessageFilterBase, public message_filters::SimpleFilter<M>
126 {
127 public:
129  typedef message_filters::MessageEvent<M const> MEvent;
130  // typedef std::function<void(const MConstPtr&, FilterFailureReason)> FailureCallback;
131 
132  // If you hit this assert your message does not have a header, or does not have the HasHeader trait defined for it
133  // Actually, we need to check that the message has a header, or that it
134  // has the FrameId and Stamp traits. However I don't know how to do that
135  // so simply commenting out for now.
136  // ROS_STATIC_ASSERT(ros::message_traits::HasHeader<M>::value);
137 
147  template<typename TimeRepT = int64_t, typename TimeT = std::nano>
149  BufferT & buffer, const std::string & target_frame, uint32_t queue_size,
150  const rclcpp::Node::SharedPtr & node,
152  : MessageFilter(buffer, target_frame, queue_size, node->get_node_logging_interface(),
153  node->get_node_clock_interface(), buffer_timeout)
154  {
156  "Buffer type must implement tf2::BufferCoreInterface");
158  "Buffer type must implement tf2_ros::AsyncBufferInterface");
159  }
160 
171  template<typename TimeRepT = int64_t, typename TimeT = std::nano>
173  BufferT & buffer, const std::string & target_frame, uint32_t queue_size,
174  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging,
175  const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock,
177  : node_logging_(node_logging),
178  node_clock_(node_clock),
179  buffer_(buffer),
180  queue_size_(queue_size),
181  buffer_timeout_(buffer_timeout)
182  {
183  init();
184  setTargetFrame(target_frame);
185  }
186 
197  template<class F, typename TimeRepT = int64_t, typename TimeT = std::nano>
199  F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size,
200  const rclcpp::Node::SharedPtr & node,
202  : MessageFilter(f, buffer, target_frame, queue_size, node->get_node_logging_interface(),
203  node->get_node_clock_interface(), buffer_timeout)
204  {
205  }
206 
218  template<class F, typename TimeRepT = int64_t, typename TimeT = std::nano>
220  F & f, BufferT & buffer, const std::string & target_frame, uint32_t queue_size,
221  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr & node_logging,
222  const rclcpp::node_interfaces::NodeClockInterface::SharedPtr & node_clock,
224  : node_logging_(node_logging),
225  node_clock_(node_clock),
226  buffer_(buffer),
227  queue_size_(queue_size),
228  buffer_timeout_(buffer_timeout)
229  {
230  init();
231  setTargetFrame(target_frame);
232  connectInput(f);
233  }
234 
238  template<class F>
239  void connectInput(F & f)
240  {
241  message_connection_.disconnect();
242  message_connection_ = f.registerCallback(&MessageFilter::incomingMessage, this);
243  }
244 
249  {
250  message_connection_.disconnect();
251  clear();
252 
254  "Successful Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
255  static_cast<long long unsigned int>(successful_transform_count_),
256  static_cast<long long unsigned int>(failed_out_the_back_count_),
257  static_cast<long long unsigned int>(transform_message_count_),
258  static_cast<long long unsigned int>(incoming_message_count_),
259  static_cast<long long unsigned int>(dropped_message_count_));
260  }
261 
265  void setTargetFrame(const std::string & target_frame)
266  {
267  V_string frames;
268  frames.push_back(target_frame);
269  setTargetFrames(frames);
270  }
271 
275  void setTargetFrames(const V_string & target_frames)
276  {
277  std::unique_lock<std::mutex> frames_lock(target_frames_mutex_);
278 
279  target_frames_.resize(target_frames.size());
280  std::transform(target_frames.begin(), target_frames.end(),
281  target_frames_.begin(), this->stripSlash);
282  expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1);
283 
285  for (V_string::iterator it = target_frames_.begin(); it != target_frames_.end(); ++it) {
286  ss << *it << " ";
287  }
288  target_frames_string_ = ss.str();
289  }
290 
295  {
296  std::unique_lock<std::mutex> frames_lock(target_frames_mutex_);
297  return target_frames_string_;
298  }
299 
303  void setTolerance(const rclcpp::Duration & tolerance)
304  {
305  std::unique_lock<std::mutex> frames_lock(target_frames_mutex_);
306  time_tolerance_ = tolerance;
307  expected_success_count_ = target_frames_.size() * (time_tolerance_.nanoseconds() ? 2 : 1);
308  }
309 
313  void clear()
314  {
315  std::unique_lock<std::mutex> unique_lock(messages_mutex_);
316 
317  TF2_ROS_MESSAGEFILTER_DEBUG("%s", "Cleared");
318 
319  messages_.clear();
320  message_count_ = 0;
321 
322  warned_about_empty_frame_id_ = false;
323  }
324 
325  void add(const MEvent & evt)
326  {
327  if (target_frames_.empty()) {
328  return;
329  }
330 
331  namespace mt = message_filters::message_traits;
332  const MConstPtr & message = evt.getMessage();
333  std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
334  rclcpp::Time stamp = mt::TimeStamp<M>::value(*message);
335 
336  if (frame_id.empty()) {
337  messageDropped(evt, filter_failure_reasons::EmptyFrameID);
338  return;
339  }
340 
341  // iterate through the target frames and add requests for each of them
342  MessageInfo info;
343  info.handles.reserve(expected_success_count_);
344  {
345  V_string target_frames_copy;
346  // Copy target_frames_ to avoid deadlock from #79
347  {
348  std::unique_lock<std::mutex> frames_lock(target_frames_mutex_);
349  target_frames_copy = target_frames_;
350  }
351 
352  V_string::iterator it = target_frames_copy.begin();
353  V_string::iterator end = target_frames_copy.end();
354  for (; it != end; ++it) {
355  const std::string & target_frame = *it;
356  auto future = buffer_.waitForTransform(
357  target_frame,
358  frame_id,
359  tf2::timeFromSec(stamp.seconds()),
360  buffer_timeout_,
361  std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, next_handle_index_));
362 
363  try {
364  const auto status = future.wait_for(std::chrono::seconds(0));
365  if (status == std::future_status::ready) {
366  future.get();
367  // Transform is available
368  ++info.success_count;
369  }
370  else {
371  info.handles.push_back(next_handle_index_++);
372  }
373  }
374  catch (const std::exception & e) {
375  TF2_ROS_MESSAGEFILTER_WARN("Message dropped because: %s", e.what());
376  messageDropped(evt, filter_failure_reasons::OutTheBack);
377  return;
378  }
379 
380  if (time_tolerance_.nanoseconds()) {
381  future = buffer_.waitForTransform(
382  target_frame,
383  frame_id,
384  tf2::timeFromSec((stamp + time_tolerance_).seconds()),
385  buffer_timeout_,
386  std::bind(&MessageFilter::transformReadyCallback, this, std::placeholders::_1, next_handle_index_));
387  try {
388  const auto status = future.wait_for(std::chrono::seconds(0));
389  if (status == std::future_status::ready) {
390  future.get();
391  // Transform is available
392  ++info.success_count;
393  }
394  else {
395  info.handles.push_back(next_handle_index_++);
396  }
397  }
398  catch (const std::exception & e) {
399  TF2_ROS_MESSAGEFILTER_WARN("Message dropped because: %s", e.what());
400  messageDropped(evt, filter_failure_reasons::OutTheBack);
401  return;
402  }
403  }
404  }
405  }
406 
407 
408  // We can transform already
409  if (info.success_count == expected_success_count_) {
410  messageReady(evt);
411  } else {
412 
413  // Keep a lock on the messages
414  std::unique_lock<std::mutex> unique_lock(messages_mutex_);
415 
416  // If this message is about to push us past our queue size, erase the oldest message
417  if (queue_size_ != 0 && message_count_ + 1 > queue_size_) {
418 
419  ++dropped_message_count_;
420  const MessageInfo & front = messages_.front();
422  "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)",
423  message_count_,
424  (mt::FrameId<M>::value(*front.event.getMessage())).c_str(),
425  mt::TimeStamp<M>::value(*front.event.getMessage()).seconds());
426 
427  messageDropped(front.event, filter_failure_reasons::Unknown);
428 
429  messages_.pop_front();
430  --message_count_;
431  }
432 
433  // Add the message to our list
434  info.event = evt;
435  messages_.push_back(info);
436  ++message_count_;
437  }
438 
439  TF2_ROS_MESSAGEFILTER_DEBUG("Added message in frame %s at time %.3f, count now %d",
440  frame_id.c_str(), stamp.seconds(), message_count_);
441  ++incoming_message_count_;
442  }
443 
449  void add(const MConstPtr & message)
450  {
451  builtin_interfaces::msg::Time t = node_clock_->get_clock()->now();
452  add(MEvent(message, t));
453  }
454 
459 #if 0
460  message_filters::Connection registerFailureCallback(const FailureCallback & callback)
461  {
462  message_connection_failure.disconnect();
463  message_connection_failure = this->registerCallback(callback, this);
464  }
465 #endif
466 
467  virtual void setQueueSize(uint32_t new_queue_size)
468  {
469  queue_size_ = new_queue_size;
470  }
471 
472  virtual uint32_t getQueueSize()
473  {
474  return queue_size_;
475  }
476 
477 private:
478  void init()
479  {
480  message_count_ = 0;
481  successful_transform_count_ = 0;
482  failed_out_the_back_count_ = 0;
483  transform_message_count_ = 0;
484  incoming_message_count_ = 0;
485  dropped_message_count_ = 0;
486  time_tolerance_ = rclcpp::Duration(0, 0);
487  warned_about_empty_frame_id_ = false;
488  expected_success_count_ = 1;
489  }
490 
491  void transformReadyCallback(const tf2_ros::TransformStampedFuture& future, const uint64_t handle)
492  {
493  namespace mt = message_filters::message_traits;
494 
495  // find the message this request is associated with
496  typename L_MessageInfo::iterator msg_it = messages_.begin();
497  typename L_MessageInfo::iterator msg_end = messages_.end();
498 
499  MEvent saved_event;
500 
501  {
502  // We will be accessing and mutating messages now, require unique lock
503  std::unique_lock<std::mutex> lock(messages_mutex_);
504 
505  for (; msg_it != msg_end; ++msg_it) {
506  MessageInfo & info = *msg_it;
507  auto handle_it = std::find(info.handles.begin(), info.handles.end(), handle);
508  if (handle_it != info.handles.end()) {
509  // found msg_it
510  ++info.success_count;
511  if (info.success_count >= expected_success_count_) {
512  saved_event = msg_it->event;
513  messages_.erase(msg_it);
514  --message_count_;
515  } else {
516  msg_it = msg_end;
517  }
518  break;
519  }
520  }
521  }
522 
523  if (msg_it == msg_end) {
524  return;
525  }
526 
527  bool can_transform = true;
528  const MConstPtr & message = saved_event.getMessage();
529  std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
530  rclcpp::Time stamp = mt::TimeStamp<M>::value(*message);
531 
532  bool transform_available = true;
533  try {
534  future.get();
535  } catch (...)
536  {
537  transform_available = false;
538  }
539 
540  if (transform_available) {
541  std::unique_lock<std::mutex> frames_lock(target_frames_mutex_);
542  // make sure we can still perform all the necessary transforms
543  typename V_string::iterator it = target_frames_.begin();
544  typename V_string::iterator end = target_frames_.end();
545  for (; it != end; ++it) {
546  const std::string & target = *it;
547  if (!buffer_.canTransform(target, frame_id, tf2::timeFromSec(stamp.seconds()), NULL)) {
548  can_transform = false;
549  break;
550  }
551 
552  if (time_tolerance_.nanoseconds()) {
553  if (!buffer_.canTransform(target, frame_id,
554  tf2::timeFromSec((stamp + time_tolerance_).seconds()), NULL))
555  {
556  can_transform = false;
557  break;
558  }
559  }
560  }
561  } else {
562  can_transform = false;
563  }
564 
565  if (can_transform) {
566  TF2_ROS_MESSAGEFILTER_DEBUG("Message ready in frame %s at time %.3f, count now %d",
567  frame_id.c_str(), stamp.seconds(), message_count_ - 1);
568 
569  ++successful_transform_count_;
570  messageReady(saved_event);
571  } else {
572  ++dropped_message_count_;
573 
574  TF2_ROS_MESSAGEFILTER_DEBUG("Discarding message in frame %s at time %.3f, count now %d",
575  frame_id.c_str(), stamp.seconds(), message_count_ - 1);
576  messageDropped(saved_event, filter_failure_reasons::Unknown);
577  }
578  }
579 
583  void incomingMessage(const message_filters::MessageEvent<M const> & evt)
584  {
585  add(evt);
586  }
587 
588  void checkFailures()
589  {
590  if (!next_failure_warning_.nanoseconds()) {
591  next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(15, 0);
592  }
593 
594  if (node_clock_->get_clock()->now() >= next_failure_warning_) {
595  if (incoming_message_count_ - message_count_ == 0) {
596  return;
597  }
598 
599  double dropped_pct = static_cast<double>(dropped_message_count_) /
600  static_cast<double>(incoming_message_count_ - message_count_);
601  if (dropped_pct > 0.95) {
603  "Dropped %.2f%% of messages so far. Please turn the [%s.message_notifier] rosconsole logger to DEBUG for more information.", dropped_pct * 100,
604  "tf2_ros_message_filter");
605  next_failure_warning_ = node_clock_->get_clock()->now() + rclcpp::Duration(60, 0);
606 
607  if (static_cast<double>(failed_out_the_back_count_) / static_cast<double>(dropped_message_count_) > 0.5) {
609  " The majority of dropped messages were due to messages growing older than the TF cache time. The last message's timestamp was: %f, and the last frame_id was: %s",
610  last_out_the_back_stamp_.seconds(), last_out_the_back_frame_.c_str());
611  }
612  }
613  }
614  }
615 
616  // TODO(clalancette): reenable this once we have underlying support for callback queues
617 #if 0
618  struct CBQueueCallback : public ros::CallbackInterface
619  {
620  CBQueueCallback(MessageFilter* filter, const MEvent& event, bool success, FilterFailureReason reason)
621  : event_(event)
622  , filter_(filter)
623  , reason_(reason)
624  , success_(success)
625  {}
626 
627 
628  virtual CallResult call()
629  {
630  if (success_)
631  {
632  filter_->signalMessage(event_);
633  }
634  else
635  {
636  filter_->signalFailure(event_, reason_);
637  }
638 
639  return Success;
640  }
641 
642  private:
643  MEvent event_;
644  MessageFilter* filter_;
645  FilterFailureReason reason_;
646  bool success_;
647  };
648 #endif
649 
650  void messageDropped(const MEvent& evt, FilterFailureReason reason)
651  {
652  // TODO(clalancette): reenable this once we have underlying support for callback queues
653 #if 0
654  if (callback_queue_) {
655  ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, false, reason));
656  callback_queue_->addCallback(cb, (uint64_t)this);
657  }
658  else
659 #endif
660  {
661  signalFailure(evt, reason);
662  }
663  }
664 
665  void messageReady(const MEvent & evt)
666  {
667  // TODO(clalancette): reenable this once we have underlying support for callback queues
668 #if 0
669  if (callback_queue_) {
670  ros::CallbackInterfacePtr cb(new CBQueueCallback(this, evt, true, filter_failure_reasons::Unknown));
671  callback_queue_->addCallback(cb, (uint64_t)this);
672  }
673  else
674 #endif
675  {
676  this->signalMessage(evt);
677  }
678  }
679 
680  void signalFailure(const MEvent & evt, FilterFailureReason reason)
681  {
682  namespace mt = message_filters::message_traits;
683  const MConstPtr & message = evt.getMessage();
684  std::string frame_id = stripSlash(mt::FrameId<M>::value(*message));
685  rclcpp::Time stamp = mt::TimeStamp<M>::value(*message);
686  RCLCPP_INFO(
687  node_logging_->get_logger(),
688  "Message Filter dropping message: frame '%s' at time %.3f for reason '%s'",
689  frame_id.c_str(), stamp.seconds(), get_filter_failure_reason_string(reason).c_str());
690  }
691 
692  static std::string stripSlash(const std::string & in)
693  {
694  if (!in.empty() && (in [0] == '/')) {
695  std::string out = in;
696  out.erase(0, 1);
697  return out;
698  }
699 
700  return in;
701  }
702 
704  const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
706  const rclcpp::node_interfaces::NodeClockInterface::SharedPtr node_clock_;
708  BufferT & buffer_;
710  V_string target_frames_;
711  std::string target_frames_string_;
713  std::mutex target_frames_mutex_;
715  uint32_t queue_size_;
716 
717  uint64_t next_handle_index_ = 0;
718  struct MessageInfo
719  {
720  MessageInfo()
721  : success_count(0) {}
722 
723  MEvent event;
724  std::vector<uint64_t> handles;
725  uint64_t success_count;
726  };
727  typedef std::list<MessageInfo> L_MessageInfo;
728  L_MessageInfo messages_;
729 
731  uint64_t message_count_;
733  std::mutex messages_mutex_;
734  uint64_t expected_success_count_;
735 
736  bool warned_about_empty_frame_id_;
737 
738  uint64_t successful_transform_count_;
739  uint64_t failed_out_the_back_count_;
740  uint64_t transform_message_count_;
741  uint64_t incoming_message_count_;
742  uint64_t dropped_message_count_;
743 
744  rclcpp::Time last_out_the_back_stamp_;
745  std::string last_out_the_back_frame_;
746 
747  rclcpp::Time next_failure_warning_;
748 
750  rclcpp::Duration time_tolerance_ = rclcpp::Duration(0, 0);
751 
752  message_filters::Connection message_connection_;
753  message_filters::Connection message_connection_failure;
754 
755  // Timeout duration when calling the buffer method 'waitForTransform'
756  tf2::Duration buffer_timeout_;
757 };
758 
759 } // namespace tf2
760 
761 #endif
std::shared_future
std::vector::resize
T resize(T... args)
std::bind
T bind(T... args)
std::string
std::shared_ptr
tf2_ros::MessageFilter::MEvent
message_filters::MessageEvent< M const > MEvent
Definition: message_filter.h:129
tf2_ros::MessageFilter
Follows the patterns set by the message_filters package to implement a filter which only passes messa...
Definition: message_filter.h:125
std::exception
std::list< MessageInfo >
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:275
std::list::pop_front
T pop_front(T... args)
std::vector
std::find
T find(T... args)
std::vector::size
T size(T... args)
tf2_ros::MessageFilter::MessageFilter
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, std::chrono::duration< TimeRepT, TimeT > buffer_timeout=std::chrono::duration< TimeRepT, TimeT >::max())
Constructor.
Definition: message_filter.h:219
tf2_ros::filter_failure_reasons::FilterFailureReason
FilterFailureReason
Definition: message_filter.h:65
std::chrono::duration
std::stringstream
std::shared_future::get
T get(T... args)
tf2_ros::MessageFilter::setQueueSize
virtual void setQueueSize(uint32_t new_queue_size)
Register a callback to be called when a message is about to be dropped.
Definition: message_filter.h:467
std::list::front
T front(T... args)
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:239
tf2_ros::MessageFilter::MessageFilter
MessageFilter(BufferT &buffer, const std::string &target_frame, uint32_t queue_size, const rclcpp::Node::SharedPtr &node, std::chrono::duration< TimeRepT, TimeT > buffer_timeout=std::chrono::duration< TimeRepT, TimeT >::max())
Constructor.
Definition: message_filter.h:148
tf2_ros::MessageFilter::getQueueSize
virtual uint32_t getQueueSize()
Definition: message_filter.h:472
std::list::clear
T clear(T... args)
tf2_ros::MessageFilter::MConstPtr
std::shared_ptr< M const > MConstPtr
Definition: message_filter.h:128
std::list::push_back
T push_back(T... args)
TF2_ROS_MESSAGEFILTER_WARN
#define TF2_ROS_MESSAGEFILTER_WARN(fmt,...)
Definition: message_filter.h:55
tf2_ros::MessageFilter::setTolerance
void setTolerance(const rclcpp::Duration &tolerance)
Set the required tolerance for the notifier to return true.
Definition: message_filter.h:303
tf2_ros
Definition: async_buffer_interface.h:41
tf2_ros::MessageFilter::getTargetFramesString
std::string getTargetFramesString()
Get the target frames as a string for debugging.
Definition: message_filter.h:294
tf2_ros::MessageFilterBase
Definition: message_filter.h:96
tf2_ros::MessageFilterBase::setTolerance
virtual void setTolerance(const rclcpp::Duration &tolerance)=0
std::string::c_str
T c_str(T... args)
std::unique_lock
tf2_ros::filter_failure_reasons::FilterFailureReasonCount
@ FilterFailureReasonCount
Max enum value for iteration, keep it at the end of the enum.
Definition: message_filter.h:76
tf2_ros::MessageFilter::~MessageFilter
~MessageFilter()
Destructor.
Definition: message_filter.h:248
tf2_ros::filter_failure_reasons::EmptyFrameID
@ EmptyFrameID
The frame_id on the message is empty.
Definition: message_filter.h:74
TF2_ROS_MESSAGEFILTER_DEBUG
#define TF2_ROS_MESSAGEFILTER_DEBUG(fmt,...)
Definition: message_filter.h:50
tf2_ros::filter_failure_reasons::Unknown
@ Unknown
The message buffer overflowed, and this message was pushed off the back of the queue,...
Definition: message_filter.h:70
tf2_ros::MessageFilter::add
void add(const MEvent &evt)
Definition: message_filter.h:325
tf2_ros::MessageFilter::MessageFilter
MessageFilter(F &f, BufferT &buffer, const std::string &target_frame, uint32_t queue_size, const rclcpp::Node::SharedPtr &node, std::chrono::duration< TimeRepT, TimeT > buffer_timeout=std::chrono::duration< TimeRepT, TimeT >::max())
Constructor.
Definition: message_filter.h:198
std::list::erase
T erase(T... args)
tf2_ros::MessageFilterBase::clear
virtual void clear()=0
tf2_ros::FilterFailureReason
filter_failure_reasons::FilterFailureReason FilterFailureReason
Definition: message_filter.h:94
tf2_ros::filter_failure_reasons::OutTheBack
@ OutTheBack
The timestamp on the message is more than the cache length earlier than the newest data in the transf...
Definition: message_filter.h:72
std::transform
T transform(T... args)
buffer.h
tf2_ros::MessageFilterBase::V_string
std::vector< std::string > V_string
Definition: message_filter.h:99
std::vector::begin
T begin(T... args)
tf2_ros::MessageFilter::setTargetFrame
void setTargetFrame(const std::string &target_frame)
Set the frame you need to be able to transform to before getting a message callback.
Definition: message_filter.h:265
tf2_ros::MessageFilter::add
void add(const MConstPtr &message)
Manually add a message into this filter.
Definition: message_filter.h:449
tf2_ros::MessageFilterBase::~MessageFilterBase
virtual ~MessageFilterBase()
Definition: message_filter.h:101
std::vector::empty
T empty(T... args)
std::mutex
std::stringstream::str
T str(T... args)
std::vector::end
T end(T... args)
tf2_ros::MessageFilterBase::setTargetFrame
virtual void setTargetFrame(const std::string &target_frame)=0
tf2_ros::MessageFilter::MessageFilter
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, std::chrono::duration< TimeRepT, TimeT > buffer_timeout=std::chrono::duration< TimeRepT, TimeT >::max())
Constructor.
Definition: message_filter.h:172
tf2_ros::MessageFilter::clear
void clear()
Clear any messages currently in the queue.
Definition: message_filter.h:313
std::exception::what
T what(T... args)
tf2_ros::MessageFilterBase::setTargetFrames
virtual void setTargetFrames(const V_string &target_frames)=0
std::is_base_of