Class TimestampManager

Nested Relationships

Nested Types

Class Documentation

class TimestampManager

A utility class that manages the set of timestamps that have been used to generate motion model constraints.

This class keeps track of the timestamps of the previously generated motion model chain, and determines the required start/stop timestamp pairs needed to update the motion model chain to span the queried time range. If the queried time range does not line up with existing constraints, this class with mark some prior constraints for removal and will generate new start/stop timestamp pairs that split the previous range and the requested time. All of the details and bookkeeping needed for creating a proper motion model implementation are handled by this class; derived motion models simply need to include a TimestampManager object and provide a function capable of generating motion model constraints between arbitrary timestamps.

Public Types

using MotionModelFunction = std::function<void(const rclcpp::Time &beginning_stamp, const rclcpp::Time &ending_stamp, std::vector<Constraint::SharedPtr> &constraints, std::vector<Variable::SharedPtr> &variables)>

Function that generates motion model constraints between the requested timestamps.

If the motion models cannot be generated for whatever reason, this function should throw an exception.

Param beginning_stamp:

[in] The beginning timestamp of the motion model constraints to be generated. beginning_stamp is guaranteed to be less than ending_stamp.

Param ending_stamp:

[in] The ending timestamp of the motion model constraints to be generated. ending_stamp is guaranteed to be greater than beginning_stamp.

Param constraints:

[out] One or more motion model constraints between the requested timestamps.

Param variables:

[out] One or more variables at both the beginning_stamp and ending_stamp. The variables should include initial values for the optimizer.

using const_stamp_range = boost::any_range<const rclcpp::Time, boost::forward_traversal_tag>

A range of timestamps.

An object representing a range defined by two iterators. It has begin() and end() methods (which means it can be used in range-based for loops), an empty() method, and a front() method for directly accessing the first member. When dereferenced, an iterator returns a const rclcpp::Time&.

Public Functions

explicit TimestampManager(MotionModelFunction generator, const rclcpp::Duration &buffer_length = rclcpp::Duration::max())

Constructor that accepts the motion model generator as a std::function object, probably constructed using std::bind()

Parameters:

  • generator – [in] A function that generates motion model constraints between the requested timestamps.

  • buffer_length – [in] The length of the motion model history. If queries arrive involving timestamps that are older than the buffer length, an exception will be thrown.

template<class T>
TimestampManager(void (T::* fp)(const rclcpp::Time &beginning_stamp, const rclcpp::Time &ending_stamp, std::vector<Constraint::SharedPtr> &constraints, std::vector<Variable::SharedPtr> &variables), T *obj, const rclcpp::Duration &buffer_length = rclcpp::Duration::max())

Constructor that accepts the motion model generator as a member function pointer and object pointer.

i.e. TimestampManager(&SomeClass::generatorMethod, &some_class_instance)

Parameters:

  • fp – [in] A function pointer to a class method that generates motion model constraints between the requested timestamps.

  • obj – [in] A pointer to the object to be used to generate motion model constraints

  • buffer_length – [in] The length of the motion model history. If queries arrive involving timestamps that are older than the buffer length, an exception will be thrown.

template<class T>
TimestampManager(void (T::* fp)(const rclcpp::Time &beginning_stamp, const rclcpp::Time &ending_stamp, std::vector<Constraint::SharedPtr> &constraints, std::vector<Variable::SharedPtr> &variables) const, T *obj, const rclcpp::Duration &buffer_length = rclcpp::Duration::max())

Constructor that accepts the motion model generator as a const member function pointer and object pointer.

i.e. TimestampManager(&SomeClass::constGeneratorMethod, &some_class_instance)

Parameters:

  • fp – [in] A function pointer to class method that generates motion model constraints between the requested timestamps.

  • obj – [in] A pointer to the object to be used to generate motion model constraints

  • buffer_length – [in] The length of the motion model history. If queries arrive involving timestamps that are older than the buffer length, an exception will be thrown.

virtual ~TimestampManager() = default

Destructor.

inline const rclcpp::Duration &bufferLength() const

Read-only access to the buffer length.

inline void bufferLength(const rclcpp::Duration &buffer_length)

Write access to the buffer length.

inline void clear()

Clear all timestamps from the motion model history.

void query(Transaction &transaction, bool update_variables = false)

Update a transaction structure such that the involved timestamps are connected by motion model constraints.

This is not as straightforward as it would seem. Depending on the history of previously generated constraints, fulfilling the request may require removing previously generated constraints and creating several new constraints, such that all timestamps are linked together in a sequential chain. This function calls the generator function provided in the constructor to generate the correct set of constraints based on history. This function is designed to be used in the derived MotionModel::applyCallback() implementation &#8212; however this method may throw an exception if it is unable to generate the requested motion models.

Parameters:

  • transaction – [inout] The transaction object that should be augmented with motion model constraints

  • update_variables – [in] Update the values of any existing variables with the newly generated values

Throws:

std::invalid_argument – If timestamps are not within the defined buffer length of the motion model

const_stamp_range stamps() const

Read-only access to the current set of timestamps.

Returns:

An iterator range containing all known timestamps in ascending order

Protected Types

using MotionModelHistory = std::map<rclcpp::Time, MotionModelSegment>

The set of previously generated motion model segments, sorted by beginning time.

The MotionModelHistory will always contain all represented timestamps; the very last entry will be the ending time of the previous MotionModelSegment, and the very last entry will be an empty MotionModelSegment.

Protected Functions

void addSegment(const rclcpp::Time &beginning_stamp, const rclcpp::Time &ending_stamp, Transaction &transaction)

Create a new MotionModelSegment, updating the provided transaction.

The motion_model_history_ container will be updated.

Parameters:

  • beginning_stamp – [in] The beginning timestamp of the new segment

  • ending_stamp – [in] The ending timestamp of the new segment

  • transaction – [out] A transaction object to be updated with the changes caused by addSegment

void removeSegment(MotionModelHistory::iterator &iter, Transaction &transaction)

Remove an existing MotionModelSegment, updating the provided transaction.

The motion_model_history_ container will be updated.

Parameters:

  • iter – [in] An iterator to the MotionModelSegment to remove. The iterator will be invalid afterwards.

  • transaction – [out] A transaction object to be updated with the changes caused by removeSegment

void splitSegment(MotionModelHistory::iterator &iter, const rclcpp::Time &stamp, Transaction &transaction)

Split an existing MotionModelSegment into two pieces at the provided timestamp, updating the provided transaction.

The motion_model_history_ container will be updated.

Parameters:

  • iter – [in] An iterator to the MotionModelSegment to split. The iterator will be invalid afterwards.

  • stamp – [in] The timestamp where the MotionModelSegment should be split

  • transaction – [out] A transaction object to be updated with the changes caused by splitSegment

void purgeHistory()

Remove any motion model segments that are older than buffer_length_.

Protected Attributes

MotionModelFunction generator_

Users upplied function that generates motion model constraints

rclcpp::Duration buffer_length_

The length of the motion model history. Segments older than buffer_length_ will be removed from the motion model history

MotionModelHistory motion_model_history_

Container that stores all previously generated motion models

struct MotionModelSegment

Structure used to represent a previously generated motion model constraint.

Public Functions

inline explicit MotionModelSegment(rcl_clock_type_t clock_t)
inline MotionModelSegment(const rclcpp::Time &beginning_stamp, const rclcpp::Time &ending_stamp, const std::vector<Constraint::SharedPtr> &constraints, const std::vector<Variable::SharedPtr> &variables)

Public Members

rclcpp::Time beginning_stamp
rclcpp::Time ending_stamp
std::vector<Constraint::SharedPtr> constraints
std::vector<Variable::SharedPtr> variables