Go to the documentation of this file.
32 #ifndef TF_MESSAGE_FILTER_H
33 #define TF_MESSAGE_FILTER_H
37 #include <tf/tfMessage.h>
42 #include <boost/function.hpp>
43 #include <boost/bind.hpp>
44 #include <boost/shared_ptr.hpp>
45 #include <boost/weak_ptr.hpp>
46 #include <boost/thread.hpp>
47 #include <boost/signals2.hpp>
54 #define TF_MESSAGEFILTER_DEBUG(fmt, ...) \
55 ROS_DEBUG_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__)
57 #define TF_MESSAGEFILTER_WARN(fmt, ...) \
58 ROS_WARN_NAMED("message_filter", "MessageFilter [target=%s]: " fmt, getTargetFramesString().c_str(), __VA_ARGS__)
63 namespace filter_failure_reasons
81 virtual void clear() = 0;
83 virtual void setTargetFrames(
const std::vector<std::string>& target_frames) = 0;
85 virtual void setQueueSize( uint32_t new_queue_size ) = 0;
180 TF_MESSAGEFILTER_DEBUG(
"Successful Transforms: %llu, Failed Transforms: %llu, Discarded due to age: %llu, Transform messages received: %llu, Messages received: %llu, Total dropped: %llu",
192 std::vector<std::string> frames;
193 frames.push_back(target_frame);
207 std::stringstream ss;
261 "Removed oldest message because buffer is full, count now %d (frame_id=%s, stamp=%f)",
277 "Added message in frame %s at time %.3f, count now %d",
294 (*header)[
"callerid"] =
"unknown";
349 if (frame_id.empty())
354 TF_MESSAGEFILTER_WARN(
"Discarding message from [%s] due to empty frame_id. This message will only print once.", callerid.c_str());
365 const std::string& target_frame = *target_it;
367 if (target_frame != frame_id && stamp !=
ros::Time(0))
378 "Discarding Message, in frame %s, Out of the back of Cache Time "
379 "(stamp: %.3f + cache_length: %.3f < latest_transform_time %.3f. "
380 "Message Count now: %d",
398 std::string& target_frame = *target_it;
435 typename L_Event::iterator it =
messages_.begin();
494 if (dropped_pct > 0.95)
volatile bool new_transforms_
Used to skip waiting on new_data_ if new transforms have come in while calling back or transforming d...
L_Event messages_
The message list.
boost::mutex failure_signal_mutex_
virtual void setTargetFrame(const std::string &target_frame)=0
boost::signals2::connection getBoostConnection() const
virtual ~MessageFilterBase()
virtual uint32_t getQueueSize()
bool warned_about_empty_frame_id_
uint64_t incoming_message_count_
#define TF_MESSAGEFILTER_DEBUG(fmt,...)
boost::mutex messages_mutex_
The mutex used for locking message list operations.
boost::signals2::connection tf_connection_
std::vector< std::string > target_frames_
The frames we need to be able to transform to before a message is ready.
void setTolerance(const ros::Duration &tolerance)
Set the required tolerance for the notifier to return true.
uint64_t failed_transform_count_
#define TF_MESSAGEFILTER_WARN(fmt,...)
~MessageFilter()
Destructor.
void connectInput(F &f)
Connect this filter's input to another filter's output. If this filter is already connected,...
const std::string & getPublisherName() const
uint32_t queue_size_
The maximum number of messages we queue up.
void disconnectFailure(const message_filters::Connection &c)
ros::MessageEvent< M const > MEvent
filter_failure_reasons::FilterFailureReason FilterFailureReason
message_filters::Connection registerFailureCallback(const FailureCallback &callback)
Register a callback to be called when a message is about to be dropped.
boost::mutex target_frames_string_mutex_
uint64_t dropped_message_count_
boost::signals2::signal< void(const MConstPtr &, FilterFailureReason)> FailureSignal
Transformer & tf_
The Transformer used to determine if transformation data is available.
uint32_t message_count_
The number of messages in the list. Used because messages_.size() has linear cost.
ros::Time last_out_the_back_stamp_
std::string getTargetFramesString()
Get the target frames as a string for debugging.
@ OutTheBack
The timestamp on the message is more than the cache length earlier than the newest data in the transf...
void setTargetFrames(const std::vector< std::string > &target_frames)
Set the frames you need to be able to transform to before getting a message callback.
FailureSignal failure_signal_
MessageFilter(F &f, Transformer &tf, const std::string &target_frame, uint32_t queue_size, ros::NodeHandle nh=ros::NodeHandle(), ros::Duration max_rate=ros::Duration(0.01))
Constructor.
uint64_t successful_transform_count_
ros::NodeHandle nh_
The node used to subscribe to the topic.
ros::Time next_failure_warning_
void maxRateTimerCallback(const ros::TimerEvent &)
void add(const MEvent &evt)
boost::function< void(const MConstPtr &, FilterFailureReason)> FailureCallback
std::list< MEvent > L_Event
virtual void setTargetFrames(const std::vector< std::string > &target_frames)=0
bool warned_about_unresolved_name_
virtual void setQueueSize(uint32_t new_queue_size)
@ EmptyFrameID
The frame_id on the message is empty.
ros::Timer max_rate_timer_
std::string target_frames_string_
void add(const MConstPtr &message)
Manually add a message into this filter.
void signalFailure(const MEvent &evt, FilterFailureReason reason)
bool new_messages_
Used to skip waiting on new_data_ if new messages have come in while calling back.
void incomingMessage(const ros::MessageEvent< M const > &evt)
Callback that happens when we receive a message on the message topic.
std::string last_out_the_back_frame_
boost::shared_ptr< M const > MConstPtr
void setTargetFrame(const std::string &target_frame)
Set the frame you need to be able to transform to before getting a message callback.
ros::Duration time_tolerance_
Provide additional tolerance on time for messages which are stamped but can have associated duration.
Follows the patterns set by the message_filters package to implement a filter which only passes messa...
#define ROS_WARN_NAMED(name,...)
MessageFilter(Transformer &tf, const std::string &target_frame, uint32_t queue_size, ros::NodeHandle nh=ros::NodeHandle(), ros::Duration max_rate=ros::Duration(0.01))
Constructor.
void clear()
Clear any messages currently in the queue.
bool testMessage(const MEvent &evt)
virtual void setTolerance(const ros::Duration &tolerance)=0
boost::shared_ptr< M > getMessage() const
#define ROSCONSOLE_DEFAULT_NAME
virtual uint32_t getQueueSize()=0
void signalMessage(const MConstPtr &msg)
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
uint64_t failed_out_the_back_count_
@ Unknown
The message buffer overflowed, and this message was pushed off the back of the queue,...
uint64_t transform_message_count_
message_filters::Connection message_connection_
virtual void setQueueSize(uint32_t new_queue_size)=0
tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sat Aug 19 2023 02:38:07