FKIE Message Filters
Improved filters for processing ROS messages
Public Types | Public Member Functions | Static Public Attributes | Protected Member Functions | List of all members
fkie_message_filters::TfFilter< Inputs > Class Template Reference

Wait for TF transformations for incoming messages. More...

#include <fkie_message_filters/tf_filter.h>

Inheritance diagram for fkie_message_filters::TfFilter< Inputs >:
Inheritance graph
[legend]
Collaboration diagram for fkie_message_filters::TfFilter< Inputs >:
Collaboration graph
[legend]

Public Types

using FilterFailureCB = std::function< void(const Inputs &..., TfFilterResult)>
 Callback for failed transform queries.
 
using Input = IO< Inputs... >
 Grouped input types. More...
 
using Output = IO< Outputs... >
 Grouped output types. More...
 

Public Member Functions

 TfFilter () noexcept
 Empty constructor. More...
 
 TfFilter (tf2::BufferCore &bc, const std::string &target_frame, uint32_t queue_size, const ros::NodeHandle &nh) noexcept
 Construct and initialize the filter. More...
 
 TfFilter (tf2::BufferCore &bc, const std::string &target_frame, uint32_t queue_size, ros::CallbackQueueInterface *cbq) noexcept
 Construct and initialize the filter. More...
 
 TfFilter (tf2::BufferCore &bc, const ros::V_string &target_frames, uint32_t queue_size, const ros::NodeHandle &nh) noexcept
 Construct and initialize the filter. More...
 
 TfFilter (tf2::BufferCore &bc, const ros::V_string &target_frames, uint32_t queue_size, ros::CallbackQueueInterface *cbq) noexcept
 Construct and initialize the filter. More...
 
Connection connect_to_sink (Sink< Outputs... > &dst) noexcept
 Connect this source to a sink. More...
 
Connection connect_to_source (Source< Inputs... > &src) noexcept
 Connect this sink to a source. More...
 
void disconnect_from_all_sinks () noexcept
 Disconnect from all connected sinks. More...
 
void disconnect_from_all_sources () noexcept
 Disconnect from all connected sources. More...
 
void init (tf2::BufferCore &bc, uint32_t queue_size, const ros::NodeHandle &nh) noexcept
 Initialize the filter. More...
 
void init (tf2::BufferCore &bc, uint32_t queue_size, ros::CallbackQueueInterface *cbq) noexcept
 Initialize the filter. More...
 
void reset () noexcept override
 Reset filter state. More...
 
void set_filter_failure_function (FilterFailureCB cb)
 Register callback for failed transforms. More...
 
void set_target_frame (const std::string &target_frame)
 Choose the TF target frame. More...
 
void set_target_frames (const ros::V_string &target_frames)
 Choose the TF target frames. More...
 

Static Public Attributes

static constexpr std::size_t NUM_INPUTS = sizeof...(Inputs)
 Number of input arguments.
 
static constexpr std::size_t NUM_OUTPUTS
 Number of output arguments.
 

Protected Member Functions

void receive (const Inputs &... in) override
 Process incoming data. More...
 
void send (const Outputs &... out)
 Pass data to all connected sinks. More...
 

Detailed Description

template<class... Inputs>
class fkie_message_filters::TfFilter< Inputs >

Wait for TF transformations for incoming messages.

This filter is intended to be used with a Subscriber as source, and will delay incoming messages until they can be transformed to the specified TF target frames. If the filter input is not unary, only the first argument is examined, which must have an accessible ROS header as determined by the ros::message_traits template.

namespace mf = fkie_message_filters;
mf::Subscriber<M> sub;
mf::TfFilter<mf::Subscriber<M>::Output> flt;
ros::NodeHandle nh;
sub.subscribe(nh, "topic", 10);
flt.init(tf2_buffer, "target_frame", 10, nh);
mf::chain(sub, flt);

Member Typedef Documentation

◆ Input

template<typename... Inputs>
using fkie_message_filters::Sink< Inputs >::Input = IO<Inputs...>
inherited

Grouped input types.

This type can be used to define sources with matching types.

◆ Output

using fkie_message_filters::Source< Outputs >::Output = IO<Outputs...>
inherited

Grouped output types.

This type can be used to define sinks with matching types.

Constructor & Destructor Documentation

◆ TfFilter() [1/5]

template<class... Inputs>
fkie_message_filters::TfFilter< Inputs >::TfFilter ( )
inlinenoexcept

Empty constructor.

Constructs an uninitialized filter object. You need to call init() before you can use the object.

◆ TfFilter() [2/5]

template<class... Inputs>
fkie_message_filters::TfFilter< Inputs >::TfFilter ( tf2::BufferCore &  bc,
const std::string &  target_frame,
uint32_t  queue_size,
const ros::NodeHandle &  nh 
)
noexcept

Construct and initialize the filter.

The constructor calls init() and set_target_frame() for you.

  • bc a TF2 buffer instance
  • target_frame the TF target frame for the incoming messages
  • queue_size the maximum number of queued messages
  • nh the ROS node handle whose callback queue is used to pass transformable messages
Exceptions
Does not throw any exceptions.

◆ TfFilter() [3/5]

template<class... Inputs>
fkie_message_filters::TfFilter< Inputs >::TfFilter ( tf2::BufferCore &  bc,
const std::string &  target_frame,
uint32_t  queue_size,
ros::CallbackQueueInterface *  cbq 
)
noexcept

Construct and initialize the filter.

The constructor calls init() and set_target_frame() for you.

  • bc a TF2 buffer instance
  • target_frame the TF target frame for the incoming messages
  • queue_size the maximum number of queued messages
  • cbq the ROS callback queue that is used to pass transformable messages. If nullptr, the send() function is called directly from the TF2 buffer callback or receive() method.
Exceptions
Does not throw any exceptions.

◆ TfFilter() [4/5]

template<class... Inputs>
fkie_message_filters::TfFilter< Inputs >::TfFilter ( tf2::BufferCore &  bc,
const ros::V_string &  target_frames,
uint32_t  queue_size,
const ros::NodeHandle &  nh 
)
noexcept

Construct and initialize the filter.

The constructor calls init() and set_target_frames() for you.

  • bc a TF2 buffer instance
  • target_frames the TF target frames for the incoming messages. Passed message will be transformable to all these frames.
  • queue_size the maximum number of queued messages
  • nh the ROS node handle whose callback queue is used to pass transformable messages
Exceptions
Does not throw any exceptions.

◆ TfFilter() [5/5]

template<class... Inputs>
fkie_message_filters::TfFilter< Inputs >::TfFilter ( tf2::BufferCore &  bc,
const ros::V_string &  target_frames,
uint32_t  queue_size,
ros::CallbackQueueInterface *  cbq 
)
noexcept

Construct and initialize the filter.

The constructor calls init() and set_target_frames() for you.

  • bc a TF2 buffer instance
  • target_frames the TF target frames for the incoming messages. Passed message will be transformable to all these frames.
  • queue_size the maximum number of queued messages
  • cbq the ROS callback queue that is used to pass transformable messages. If nullptr, the send() function is called directly from the TF2 buffer callback or receive() method.
Exceptions
Does not throw any exceptions.

Member Function Documentation

◆ connect_to_sink()

Connection fkie_message_filters::Source< Outputs >::connect_to_sink ( Sink< Outputs... > &  dst)
noexceptinherited

Connect this source to a sink.

Can be called multiple times to connect multiple sinks; in that case, the sinks receive data in the same order as they have been connected. This function does basically the same thing as Sink::connect_to_source(), only from the opposite point of view.

  • dst the sink that is to be connected
Returns
a connection object that can be used to monitor or sever the created connection
Exceptions
Does not throw any exceptions.

◆ connect_to_source()

template<typename... Inputs>
Connection fkie_message_filters::Sink< Inputs >::connect_to_source ( Source< Inputs... > &  src)
noexceptinherited

Connect this sink to a source.

Can be called multiple times to connect multiple sources; in that case, the sink receives data from all connected sources. This function does basically the same thing as Source::connect_to_sink(), only from the opposite point of view.

  • src the source that is to be connected
Returns
a connection object that can be used to monitor or sever the created connection
Exceptions
Does not throw any exceptions.

◆ disconnect_from_all_sinks()

void fkie_message_filters::Source< Outputs >::disconnect_from_all_sinks ( )
noexceptinherited

Disconnect from all connected sinks.

Severs the connection to all sinks, turning the send() method into a no-op.

Exceptions
Does not throw any exceptions.

◆ disconnect_from_all_sources()

template<typename... Inputs>
void fkie_message_filters::Sink< Inputs >::disconnect_from_all_sources ( )
noexceptinherited

Disconnect from all connected sources.

Severs the connection to all sources. The receive() method will not be called any more.

Exceptions
Does not throw any exceptions.

◆ init() [1/2]

template<class... Inputs>
void fkie_message_filters::TfFilter< Inputs >::init ( tf2::BufferCore &  bc,
uint32_t  queue_size,
const ros::NodeHandle &  nh 
)
noexcept

Initialize the filter.

This function allocates internal data structures and makes the filter operational. If the function is called on an already initialized filter, the filter is reinitialized and reset() is called implicitly.

  • bc a TF2 buffer instance
  • queue_size the maximum number of queued messages
  • nh the ROS node handle whose callback queue is used to pass transformable messages
Exceptions
Does not throw any exceptions.

◆ init() [2/2]

template<class... Inputs>
void fkie_message_filters::TfFilter< Inputs >::init ( tf2::BufferCore &  bc,
uint32_t  queue_size,
ros::CallbackQueueInterface *  cbq 
)
noexcept

Initialize the filter.

This function allocates internal data structures and makes the filter operational. If the function is called on an already initialized filter, the filter is reinitialized and reset() is called implicitly.

  • bc a TF2 buffer instance
  • queue_size the maximum number of queued messages
  • cbq the ROS callback queue that is used to pass transformable messages. If nullptr, the send() function is called directly from the TF2 buffer callback or receive() method.
Exceptions
Does not throw any exceptions.

◆ receive()

template<class... Inputs>
void fkie_message_filters::TfFilter< Inputs >::receive ( const Inputs &...  in)
overrideprotectedvirtual

Process incoming data.

Derived classes need to override this method to handle all data that is to be consumed by the sink.

Exceptions
Depends on the implementation.

Implements fkie_message_filters::Sink< Inputs >.

◆ reset()

template<class... Inputs>
void fkie_message_filters::TfFilter< Inputs >::reset ( )
overridevirtualnoexcept

Reset filter state.

Discards all queued messages. Existing connections to sources and sinks are unaffected.

Exceptions
Does not throw any exceptions.

Reimplemented from fkie_message_filters::FilterBase.

◆ send()

void fkie_message_filters::Source< Outputs >::send ( const Outputs &...  out)
protectedinherited

Pass data to all connected sinks.

  • out data
Exceptions
Does not throw any exceptions, but will propagate uncaught exceptions from filter callbacks.

◆ set_filter_failure_function()

template<class... Inputs>
void fkie_message_filters::TfFilter< Inputs >::set_filter_failure_function ( FilterFailureCB  cb)

Register callback for failed transforms.

Whenever a message is discarded, this callback is called with the message and the reason why the TF transform failed.

  • cb callback function
Exceptions
std::logic_errorif the filter is uninitialized

◆ set_target_frame()

template<class... Inputs>
void fkie_message_filters::TfFilter< Inputs >::set_target_frame ( const std::string &  target_frame)

Choose the TF target frame.

  • target_frame all passed messages will be transformable to this TF frame
Exceptions
std::logic_errorif the filter is uninitialized

◆ set_target_frames()

template<class... Inputs>
void fkie_message_filters::TfFilter< Inputs >::set_target_frames ( const ros::V_string &  target_frames)

Choose the TF target frames.

  • target_frames all passed messages will be transformable to all these TF frames
Exceptions
std::logic_errorif the filter is uninitialized

The documentation for this class was generated from the following file: