Class VariableStampIndex

Class Documentation

class VariableStampIndex

Object designed to track the timestamps associated with each variable.

If the variable is derived from a fuse_variables::Stamped type, then the variable’s stamp is used. If a variable is not derived from a fuse_variables::Stamped type, then the timestamp of the newest fuse_variables::Stamped variable directly connected to the unstamped variable is used. If an unstamped variable is not directly connected to any variable, then it is assigned a zero timestamp.

Public Functions

VariableStampIndex() = default

Constructor.

virtual ~VariableStampIndex() = default

Destructor.

inline bool empty() const

Return true if no variables exist in the index.

inline size_t size() const

Returns the number of variables in the index.

inline void clear()

Clear all tracked state.

rclcpp::Time currentStamp() const

Returns the most recent timestamp associated with any variable.

If there is no timestamp in the index, returns rclcpp::Time(0, 0, RCL_CLOCK_UNINITIALIZED)

void addNewTransaction(const fuse_core::Transaction &transaction)

Update the index with the information from the added transactions.

Parameters:

transaction – [in] The set of variables and constraints to add and remove

void addMarginalTransaction(const fuse_core::Transaction &transaction)

Update the index with the information from a marginal transaction.

Only a subset of information is used from the marginal transaction. We do not want to track the variable connections induced by marginal factors.

Parameters:

transaction – [in] The set of variables and constraints to remove

template<typename OutputUuidIterator>
inline void query(const rclcpp::Time &stamp, OutputUuidIterator result) const

Add all variables that are not directly connected to a stamped variable with a timestamp greater than or equal to the provided stamp.

Parameters:

  • stamp – [in] The reference timestamp. Only variables not associated with timestamps greater than or equal to this will be added to the output container

  • result – [out] An output iterator capable of receiving fuse_core::UUID objects

Protected Types

using StampedMap = std::unordered_map<fuse_core::UUID, rclcpp::Time>
using VariableToConstraintsMap = std::unordered_map<fuse_core::UUID, std::unordered_set<fuse_core::UUID>>
using ConstraintToVariablesMap = std::unordered_map<fuse_core::UUID, std::unordered_set<fuse_core::UUID>>

Protected Functions

void applyAddedConstraints(const fuse_core::Transaction &transaction)

Update this VariableStampIndex with the added constraints from the provided transaction.

void applyAddedVariables(const fuse_core::Transaction &transaction)

Update this VariableStampIndex with the added variables from the provided transaction.

void applyRemovedConstraints(const fuse_core::Transaction &transaction)

Update this VariableStampIndex with the removed constraints from the provided transaction.

void applyRemovedVariables(const fuse_core::Transaction &transaction)

Update this VariableStampIndex with the removed variables from the provided transaction.

Protected Attributes

StampedMap stamped_index_

Container that holds the UUID->Stamp mapping for fuse_variables::Stamped variables

VariableToConstraintsMap variables_
ConstraintToVariablesMap constraints_