Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available. More...
#include <message_filter.h>
Classes | |
struct | CBQueueCallback |
struct | MessageInfo |
Public Types | |
typedef boost::function< void(const MConstPtr &, FilterFailureReason)> | FailureCallback |
typedef boost::signal< void(const MConstPtr &, FilterFailureReason)> | FailureSignal |
typedef boost::shared_ptr< M const > | MConstPtr |
typedef ros::MessageEvent< M const > | MEvent |
Public Member Functions | |
void | add (const MEvent &evt) |
void | add (const MConstPtr &message) |
Manually add a message into this filter. | |
void | clear () |
Clear any messages currently in the queue. | |
template<class F > | |
void | connectInput (F &f) |
Connect this filter's input to another filter's output. If this filter is already connected, disconnects first. | |
std::string | getTargetFramesString () |
Get the target frames as a string for debugging. | |
MessageFilter (tf2::BufferCore &bc, const std::string &target_frame, uint32_t queue_size, const ros::NodeHandle &nh) | |
Constructor. | |
template<class F > | |
MessageFilter (F &f, tf2::BufferCore &bc, const std::string &target_frame, uint32_t queue_size, const ros::NodeHandle &nh) | |
Constructor. | |
MessageFilter (tf2::BufferCore &bc, const std::string &target_frame, uint32_t queue_size, ros::CallbackQueueInterface *cbqueue) | |
Constructor. | |
template<class F > | |
MessageFilter (F &f, tf2::BufferCore &bc, const std::string &target_frame, uint32_t queue_size, ros::CallbackQueueInterface *cbqueue) | |
Constructor. | |
message_filters::Connection | registerFailureCallback (const FailureCallback &callback) |
Register a callback to be called when a message is about to be dropped. | |
ROS_STATIC_ASSERT (ros::message_traits::HasHeader< M >::value) | |
void | setTargetFrame (const std::string &target_frame) |
Set the frame you need to be able to transform to before getting a message callback. | |
void | setTargetFrames (const V_string &target_frames) |
Set the frames you need to be able to transform to before getting a message callback. | |
void | setTolerance (const ros::Duration &tolerance) |
Set the required tolerance for the notifier to return true. | |
~MessageFilter () | |
Destructor. | |
Private Types | |
typedef std::list< MessageInfo > | L_MessageInfo |
typedef std::vector < tf2::TransformableRequestHandle > | V_TransformableRequestHandle |
Private Member Functions | |
void | checkFailures () |
void | disconnectFailure (const message_filters::Connection &c) |
void | incomingMessage (const ros::MessageEvent< M const > &evt) |
Callback that happens when we receive a message on the message topic. | |
void | init () |
void | messageDropped (const MEvent &evt, FilterFailureReason reason) |
void | messageReady (const MEvent &evt) |
void | signalFailure (const MEvent &evt, FilterFailureReason reason) |
void | transformable (tf2::TransformableRequestHandle request_handle, const std::string &target_frame, const std::string &source_frame, ros::Time time, tf2::TransformableResult result) |
Private Attributes | |
tf2::BufferCore & | bc_ |
The Transformer used to determine if transformation data is available. | |
tf2::TransformableCallbackHandle | callback_handle_ |
ros::CallbackQueueInterface * | callback_queue_ |
uint64_t | dropped_message_count_ |
uint32_t | expected_success_count_ |
uint64_t | failed_out_the_back_count_ |
FailureSignal | failure_signal_ |
boost::mutex | failure_signal_mutex_ |
uint64_t | incoming_message_count_ |
std::string | last_out_the_back_frame_ |
ros::Time | last_out_the_back_stamp_ |
message_filters::Connection | message_connection_ |
uint32_t | message_count_ |
The number of messages in the list. Used because <container>.size() may have linear cost. | |
L_MessageInfo | messages_ |
boost::mutex | messages_mutex_ |
The mutex used for locking message list operations. | |
ros::WallTime | next_failure_warning_ |
uint32_t | queue_size_ |
The maximum number of messages we queue up. | |
uint64_t | successful_transform_count_ |
V_string | target_frames_ |
The frames we need to be able to transform to before a message is ready. | |
std::string | target_frames_string_ |
boost::mutex | target_frames_string_mutex_ |
ros::Duration | time_tolerance_ |
Provide additional tolerance on time for messages which are stamped but can have associated duration. | |
uint64_t | transform_message_count_ |
bool | warned_about_empty_frame_id_ |
Follows the patterns set by the message_filters package to implement a filter which only passes messages through once there is transform data available.
The callbacks used in this class are of the same form as those used by roscpp's message callbacks.
MessageFilter is templated on a message type.
If you want to hook a MessageFilter into a ROS topic:
message_filters::Subscriber<MessageType> sub(node_handle_, "topic", 10); tf::MessageFilter<MessageType> tf_filter(sub, tf_listener_, "/map", 10); tf_filter.registerCallback(&MyClass::myCallback, this);
Definition at line 105 of file message_filter.h.
typedef boost::function<void(const MConstPtr&, FilterFailureReason)> tf2_ros::MessageFilter< M >::FailureCallback |
Definition at line 110 of file message_filter.h.
typedef boost::signal<void(const MConstPtr&, FilterFailureReason)> tf2_ros::MessageFilter< M >::FailureSignal |
Definition at line 111 of file message_filter.h.
typedef std::list<MessageInfo> tf2_ros::MessageFilter< M >::L_MessageInfo [private] |
Definition at line 645 of file message_filter.h.
typedef boost::shared_ptr<M const> tf2_ros::MessageFilter< M >::MConstPtr |
Reimplemented from message_filters::SimpleFilter< M >.
Definition at line 108 of file message_filter.h.
typedef ros::MessageEvent<M const> tf2_ros::MessageFilter< M >::MEvent |
Definition at line 109 of file message_filter.h.
typedef std::vector<tf2::TransformableRequestHandle> tf2_ros::MessageFilter< M >::V_TransformableRequestHandle [private] |
Definition at line 634 of file message_filter.h.
tf2_ros::MessageFilter< M >::MessageFilter | ( | tf2::BufferCore & | bc, |
const std::string & | target_frame, | ||
uint32_t | queue_size, | ||
const ros::NodeHandle & | nh | ||
) | [inline] |
Constructor.
tf | The tf::Transformer this filter should use |
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. |
queue_size | The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). |
nh | The NodeHandle whose callback queue we should add callbacks to |
Definition at line 124 of file message_filter.h.
tf2_ros::MessageFilter< M >::MessageFilter | ( | F & | f, |
tf2::BufferCore & | bc, | ||
const std::string & | target_frame, | ||
uint32_t | queue_size, | ||
const ros::NodeHandle & | nh | ||
) | [inline] |
Constructor.
f | The filter to connect this filter's input to. Often will be a message_filters::Subscriber. |
tf | The tf::Transformer this filter should use |
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. |
queue_size | The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). |
nh | The NodeHandle whose callback queue we should add callbacks to |
Definition at line 144 of file message_filter.h.
tf2_ros::MessageFilter< M >::MessageFilter | ( | tf2::BufferCore & | bc, |
const std::string & | target_frame, | ||
uint32_t | queue_size, | ||
ros::CallbackQueueInterface * | cbqueue | ||
) | [inline] |
Constructor.
tf | The tf::Transformer this filter should use |
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. |
queue_size | The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). |
cbqueue | The callback queue to add callbacks to. If NULL, callbacks will happen from whatever thread either a) add() is called, which will generally be when the previous filter in the chain outputs a message, or b) tf2::BufferCore::setTransform() is called |
Definition at line 166 of file message_filter.h.
tf2_ros::MessageFilter< M >::MessageFilter | ( | F & | f, |
tf2::BufferCore & | bc, | ||
const std::string & | target_frame, | ||
uint32_t | queue_size, | ||
ros::CallbackQueueInterface * | cbqueue | ||
) | [inline] |
Constructor.
f | The filter to connect this filter's input to. Often will be a message_filters::Subscriber. |
tf | The tf::Transformer this filter should use |
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. |
queue_size | The number of messages to queue up before throwing away old ones. 0 means infinite (dangerous). |
cbqueue | The callback queue to add callbacks to. If NULL, callbacks will happen from whatever thread either a) add() is called, which will generally be when the previous filter in the chain outputs a message, or b) tf2::BufferCore::setTransform() is called |
Definition at line 188 of file message_filter.h.
tf2_ros::MessageFilter< M >::~MessageFilter | ( | ) | [inline] |
Destructor.
Definition at line 213 of file message_filter.h.
void tf2_ros::MessageFilter< M >::add | ( | const MEvent & | evt | ) | [inline] |
Definition at line 291 of file message_filter.h.
void tf2_ros::MessageFilter< M >::add | ( | const MConstPtr & | message | ) | [inline] |
Manually add a message into this filter.
Definition at line 398 of file message_filter.h.
void tf2_ros::MessageFilter< M >::checkFailures | ( | ) | [inline, private] |
Definition at line 530 of file message_filter.h.
void tf2_ros::MessageFilter< M >::clear | ( | ) | [inline, virtual] |
Clear any messages currently in the queue.
Implements tf2_ros::MessageFilterBase.
Definition at line 276 of file message_filter.h.
void tf2_ros::MessageFilter< M >::connectInput | ( | F & | f | ) | [inline] |
Connect this filter's input to another filter's output. If this filter is already connected, disconnects first.
Definition at line 204 of file message_filter.h.
void tf2_ros::MessageFilter< M >::disconnectFailure | ( | const message_filters::Connection & | c | ) | [inline, private] |
Definition at line 615 of file message_filter.h.
std::string tf2_ros::MessageFilter< M >::getTargetFramesString | ( | ) | [inline] |
Get the target frames as a string for debugging.
Definition at line 257 of file message_filter.h.
void tf2_ros::MessageFilter< M >::incomingMessage | ( | const ros::MessageEvent< M const > & | evt | ) | [inline, private] |
Callback that happens when we receive a message on the message topic.
Definition at line 525 of file message_filter.h.
void tf2_ros::MessageFilter< M >::init | ( | ) | [inline, private] |
Definition at line 419 of file message_filter.h.
void tf2_ros::MessageFilter< M >::messageDropped | ( | const MEvent & | evt, |
FilterFailureReason | reason | ||
) | [inline, private] |
Definition at line 589 of file message_filter.h.
void tf2_ros::MessageFilter< M >::messageReady | ( | const MEvent & | evt | ) | [inline, private] |
Definition at line 602 of file message_filter.h.
message_filters::Connection tf2_ros::MessageFilter< M >::registerFailureCallback | ( | const FailureCallback & | callback | ) | [inline] |
Register a callback to be called when a message is about to be dropped.
callback | The callback to call |
Definition at line 411 of file message_filter.h.
tf2_ros::MessageFilter< M >::ROS_STATIC_ASSERT | ( | ros::message_traits::HasHeader< M >::value | ) |
void tf2_ros::MessageFilter< M >::setTargetFrame | ( | const std::string & | target_frame | ) | [inline, virtual] |
Set the frame you need to be able to transform to before getting a message callback.
Implements tf2_ros::MessageFilterBase.
Definition at line 229 of file message_filter.h.
void tf2_ros::MessageFilter< M >::setTargetFrames | ( | const V_string & | target_frames | ) | [inline, virtual] |
Set the frames you need to be able to transform to before getting a message callback.
Implements tf2_ros::MessageFilterBase.
Definition at line 239 of file message_filter.h.
void tf2_ros::MessageFilter< M >::setTolerance | ( | const ros::Duration & | tolerance | ) | [inline, virtual] |
Set the required tolerance for the notifier to return true.
Implements tf2_ros::MessageFilterBase.
Definition at line 266 of file message_filter.h.
void tf2_ros::MessageFilter< M >::signalFailure | ( | const MEvent & | evt, |
FilterFailureReason | reason | ||
) | [inline, private] |
Definition at line 621 of file message_filter.h.
void tf2_ros::MessageFilter< M >::transformable | ( | tf2::TransformableRequestHandle | request_handle, |
const std::string & | target_frame, | ||
const std::string & | source_frame, | ||
ros::Time | time, | ||
tf2::TransformableResult | result | ||
) | [inline, private] |
Definition at line 434 of file message_filter.h.
tf2::BufferCore& tf2_ros::MessageFilter< M >::bc_ [private] |
The Transformer used to determine if transformation data is available.
Definition at line 627 of file message_filter.h.
tf2::TransformableCallbackHandle tf2_ros::MessageFilter< M >::callback_handle_ [private] |
Definition at line 632 of file message_filter.h.
ros::CallbackQueueInterface* tf2_ros::MessageFilter< M >::callback_queue_ [private] |
Definition at line 671 of file message_filter.h.
uint64_t tf2_ros::MessageFilter< M >::dropped_message_count_ [private] |
Definition at line 657 of file message_filter.h.
uint32_t tf2_ros::MessageFilter< M >::expected_success_count_ [private] |
Definition at line 649 of file message_filter.h.
uint64_t tf2_ros::MessageFilter< M >::failed_out_the_back_count_ [private] |
Definition at line 654 of file message_filter.h.
FailureSignal tf2_ros::MessageFilter< M >::failure_signal_ [private] |
Definition at line 668 of file message_filter.h.
boost::mutex tf2_ros::MessageFilter< M >::failure_signal_mutex_ [private] |
Definition at line 669 of file message_filter.h.
uint64_t tf2_ros::MessageFilter< M >::incoming_message_count_ [private] |
Definition at line 656 of file message_filter.h.
std::string tf2_ros::MessageFilter< M >::last_out_the_back_frame_ [private] |
Definition at line 660 of file message_filter.h.
ros::Time tf2_ros::MessageFilter< M >::last_out_the_back_stamp_ [private] |
Definition at line 659 of file message_filter.h.
message_filters::Connection tf2_ros::MessageFilter< M >::message_connection_ [private] |
Definition at line 666 of file message_filter.h.
uint32_t tf2_ros::MessageFilter< M >::message_count_ [private] |
The number of messages in the list. Used because <container>.size() may have linear cost.
Definition at line 647 of file message_filter.h.
L_MessageInfo tf2_ros::MessageFilter< M >::messages_ [private] |
Definition at line 646 of file message_filter.h.
boost::mutex tf2_ros::MessageFilter< M >::messages_mutex_ [private] |
The mutex used for locking message list operations.
Definition at line 648 of file message_filter.h.
ros::WallTime tf2_ros::MessageFilter< M >::next_failure_warning_ [private] |
Definition at line 662 of file message_filter.h.
uint32_t tf2_ros::MessageFilter< M >::queue_size_ [private] |
The maximum number of messages we queue up.
Definition at line 631 of file message_filter.h.
uint64_t tf2_ros::MessageFilter< M >::successful_transform_count_ [private] |
Definition at line 653 of file message_filter.h.
V_string tf2_ros::MessageFilter< M >::target_frames_ [private] |
The frames we need to be able to transform to before a message is ready.
Definition at line 628 of file message_filter.h.
std::string tf2_ros::MessageFilter< M >::target_frames_string_ [private] |
Definition at line 629 of file message_filter.h.
boost::mutex tf2_ros::MessageFilter< M >::target_frames_string_mutex_ [private] |
Definition at line 630 of file message_filter.h.
ros::Duration tf2_ros::MessageFilter< M >::time_tolerance_ [private] |
Provide additional tolerance on time for messages which are stamped but can have associated duration.
Definition at line 664 of file message_filter.h.
uint64_t tf2_ros::MessageFilter< M >::transform_message_count_ [private] |
Definition at line 655 of file message_filter.h.
bool tf2_ros::MessageFilter< M >::warned_about_empty_frame_id_ [private] |
Definition at line 651 of file message_filter.h.