Template Class InputAligner

Nested Relationships

Nested Types

Inheritance Relationships

Base Type

Class Documentation

template<typename ...Ms>
class InputAligner : public message_filters::noncopyable

Aligns multiple inputs in time and passing them through in order. For N inputs this filter provides N outputs. Often sensors or pre-processing chains might introduce delays to messages until they arrive at a target node. The input aligner ensures that the messages are forwarded in order.

If the input messages are periodic a hint about their frequency can be set via setInputPeriod for each input. This allows the filter to look ahead for the given time and reduces delays. The period should be selected optimistically at the lower bound.

If for an input no messages arrive the input will timeout and will be ignored until new messages arrive.

Forwarding messages can be either triggered by calling dispatchMessages periodically or by setting up a timer with setupDispatchTimer.

This implementation was inspired by ROCK’s stream aligner. https://www.rock-robotics.org/documentation/data_processing/stream_aligner.html

Public Types

typedef std::tuple<Ms...> Messages
typedef std::tuple<MessageEvent<Ms const>...> Events
typedef std::tuple<Signal1<Ms>...> Signals

Public Functions

template<class F0, class F1, class ...Fs>
inline InputAligner(const rclcpp::Duration &timeout, F0 &f0, F1 &f1, Fs&... fs)
inline explicit InputAligner(const rclcpp::Duration &timeout)
inline virtual ~InputAligner()
template<class ...Fs>
inline void connectInput(Fs&... fs)

Connects |Fs| inputs. Disconnects existing inputs.

template<std::size_t I, typename P>
inline Connection registerCallback(const std::function<void(P)> &callback)

Register a callback for the I-th input.

Parameters:

callback – The callback to call

template<std::size_t I, typename P>
inline Connection registerCallback(void (*callback)(P))

Register a callback for the I-th input.

Parameters:

callback – The callback to call

template<std::size_t I, typename T, typename P>
inline Connection registerCallback(void (T::* callback)(P), T *t)

Register a callback for the I-th input.

Parameters:

callback – The callback to call

inline void setName(const std::string &name)

Set the name of this filter. For debugging use.

inline const std::string &getName()

Get the name of this filter. For debugging use.

template<std::size_t I>
inline void add(const std::shared_ptr<typename std::tuple_element_t<I, Messages> const> &msg)

Adds a message to the I-th input queue. With this function this filter can also be used without the use of other filter classes.

template<std::size_t I>
inline void setInputPeriod(const rclcpp::Duration &period)

Sets the input period for the I-th input. This allows the filter to look ahead for the given time and reduces delays. The period should be selected optimistically at the lower bound. The default value is 0, which means there is no look ahead for this input.

template<std::size_t I>
inline QueueStatus getQueueStatus() const
template<class NodeType>
inline void setupDispatchTimer(std::shared_ptr<NodeType> node, const rclcpp::Duration &update_rate)

Sets up a timer calling the dispatchMessages function.

Parameters:

  • node – The ROS node.

  • update_rate – The rate in which dispatchMessages is triggered.

inline void dispatchMessages()

Forwards all messages that can be aligned. This function should be called periodically.

Public Static Attributes

static constexpr std::size_t N_INPUTS = sizeof...(Ms)

Protected Types

typedef std::tuple<EventQueue<Ms>...> EventQueues

Protected Functions

template<std::size_t I, class FTuple>
inline void connect(FTuple &ftuple)
template<class FTuple, std::size_t... Is>
inline void connectInputImpl(FTuple &ftuple, std::index_sequence<Is...>)
inline void disconnectAll()
template<std::size_t I>
inline void addEvent(const typename std::tuple_element_t<I, Events> &evt)
template<std::size_t... Is>
inline bool inputsAvailable(std::index_sequence<Is...>) const
template<std::size_t... Is>
inline bool dispatchFirstMessage(std::index_sequence<Is...>)
template<std::size_t I>
inline bool dispatchFirstMessage(std::size_t idx)
template<std::size_t I>
inline std::size_t getFirstSampleIdx(const std::array<rclcpp::Time, I> &ts) const

Protected Attributes

rclcpp::Duration timeout_
rclcpp::Time last_in_ts_
rclcpp::Time last_out_ts_
EventQueues event_queues_
Connection input_connections_[N_INPUTS]
Signals signals_
std::string name_
std::mutex mutex_
rclcpp::TimerBase::SharedPtr dispatch_timer_
template<typename MsgType>
class EventQueue : public std::multiset<MessageEvent<MsgType const>, EventSort<MsgType>>

Public Functions

inline EventQueue()
inline rclcpp::Time firstTimeStamp()
inline void popFirst()
inline void msgDropped()
inline void setPeriod(const rclcpp::Duration &period)
inline void setActive(bool active)
inline QueueStatus getStatus() const

Protected Attributes

rclcpp::Time next_ts_
rclcpp::Duration period_
bool active_
std::size_t msgs_processed_
std::size_t msgs_dropped_
template<typename MsgType>
struct EventSort

Public Functions

inline bool operator()(const MessageEvent<MsgType const> &lhs, const MessageEvent<MsgType const> &rhs) const
struct QueueStatus

Status information of a input queue.

Public Members

bool active
std::size_t queue_size
std::size_t msgs_processed
std::size_t msgs_dropped