Public Types | Public Member Functions | List of all members
mpc_local_planner::TimeSeriesSE2 Class Reference

Time Series with SE2 support. More...

#include <time_series_se2.h>

Inheritance diagram for mpc_local_planner::TimeSeriesSE2:
Inheritance graph
[legend]

Public Types

using ConstPtr = std::shared_ptr< const TimeSeriesSE2 >
 
using Ptr = std::shared_ptr< TimeSeriesSE2 >
 
- Public Types inherited from corbo::TimeSeries
typedef std::shared_ptr< const TimeSeriesConstPtr
 
enum  Extrapolation { Extrapolation::NoExtrapolation, Extrapolation::ZeroOrderHold }
 
enum  Interpolation { Interpolation::ZeroOrderHold, Interpolation::Linear }
 
enum  Normalization { Normalization::FirstValue, Normalization::AbsoluteMaximum, Normalization::Maximum, Normalization::Mean }
 
typedef std::shared_ptr< TimeSeriesPtr
 
typedef Eigen::Map< const Eigen::VectorXd > TimeVecConstMap
 
typedef Eigen::Map< Eigen::VectorXd > TimeVecMap
 
typedef Eigen::Map< const Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor > > ValuesMatConstMap
 
typedef Eigen::Map< Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor > > ValuesMatMap
 

Public Member Functions

void computeMeanCwise (Eigen::Ref< Eigen::VectorXd > mean_values) override
 Compute and return the component-wise mean values. More...
 
double computeMeanOverall () override
 Compute and return the mean value of all values among all dimensions. More...
 
bool getValuesInterpolate (double time, Eigen::Ref< Eigen::VectorXd > values, Interpolation interpolation=Interpolation::Linear, Extrapolation extrapolate=Extrapolation::NoExtrapolation, double tolerance=1e-6) const override
 Retrieve value vector at a desired time stamp (seconds) via interpolation. More...
 
 TimeSeriesSE2 ()=default
 Default constructor. More...
 
 TimeSeriesSE2 (int value_dim)
 Construct empty time series with a dresired value vector dimension. More...
 
- Public Member Functions inherited from corbo::TimeSeries
bool add (double time, const Eigen::Ref< const Eigen::VectorXd > &values)
 
bool add (double time, const std::vector< double > &values)
 
void clear ()
 
double getFinalTime () const
 
const std::vector< double > & getTime () const
 
int getTimeDimension () const
 
const double & getTimeFromStart () const
 
std::vector< double > & getTimeRef ()
 
TimeVecMap getTimeVectorView ()
 
TimeVecConstMap getTimeVectorView () const
 
int getValueDimension () const
 
const std::vector< std::string > & getValueLabels () const
 
std::vector< std::string > & getValueLabelsRef ()
 
const std::vector< double > & getValues () const
 
std::vector< double > getValues (int time_idx) const
 
Eigen::Map< const Eigen::VectorXd > getValuesMap (int time_idx) const
 
ValuesMatMap getValuesMatrixView ()
 
ValuesMatConstMap getValuesMatrixView () const
 
std::vector< double > & getValuesRef ()
 
bool isColumnMajor () const
 
bool isEmpty () const
 
virtual void normalize (Normalization norm_method, bool value_cwise=true)
 
virtual void normalize (Normalization norm_method, int value_idx)
 
void print (int precision=2) const
 
void reserve (int time_dim, int value_dim)
 
bool set (const Eigen::Ref< const Eigen::VectorXd > &time, const Eigen::Ref< const Eigen::MatrixXd > &values_matrix, double time_from_start=0.0)
 
bool set (const std::vector< double > &time, const std::vector< Eigen::VectorXd > &values_vector, double time_from_start=0.0)
 
void setTimeFromStart (double tfs)
 
void setValueDimension (int value_dim)
 
 TimeSeries ()
 
 TimeSeries (int value_dim)
 
virtual ~TimeSeries ()
 

Additional Inherited Members

- Protected Attributes inherited from corbo::TimeSeries
std::vector< double > _time
 
double _time_from_start
 
int _value_dim
 
std::vector< std::string > _value_labels
 
std::vector< double > _values
 

Detailed Description

Time Series with SE2 support.

A time series represents a sequence of vectors of floating point numbers associated with time stamps. A time series might also be interpreted as a discrete-time trajectory. This class extends corbo::TimeSeries to support SE2 poses. Hereby, the third state component is always treated as orientation in [-pi, pi). In particular, whenever values are retrieved by interpolation (getValuesInterpolate()), the third state is ensured to remain properly in [-pi, pi).

Note, as the first three state components define the SE2, any further auxiliary state component is treated as ordinary real number.

See also
corbo::TimeSeries
Author
Christoph Rösmann (chris.nosp@m.toph.nosp@m..roes.nosp@m.mann.nosp@m.@tu-d.nosp@m.ortm.nosp@m.und.d.nosp@m.e)

Definition at line 70 of file time_series_se2.h.

Member Typedef Documentation

◆ ConstPtr

Definition at line 94 of file time_series_se2.h.

◆ Ptr

Definition at line 93 of file time_series_se2.h.

Constructor & Destructor Documentation

◆ TimeSeriesSE2() [1/2]

mpc_local_planner::TimeSeriesSE2::TimeSeriesSE2 ( )
default

Default constructor.

◆ TimeSeriesSE2() [2/2]

mpc_local_planner::TimeSeriesSE2::TimeSeriesSE2 ( int  value_dim)
inlineexplicit

Construct empty time series with a dresired value vector dimension.

Definition at line 99 of file time_series_se2.h.

Member Function Documentation

◆ computeMeanCwise()

void mpc_local_planner::TimeSeriesSE2::computeMeanCwise ( Eigen::Ref< Eigen::VectorXd >  mean_values)
overridevirtual

Compute and return the component-wise mean values.

Parameters
[out]mean_valuesResulting cwise mean values [getValueDimension() x 1] (vector size must be preallocated)

Reimplemented from corbo::TimeSeries.

Definition at line 140 of file time_series_se2.cpp.

◆ computeMeanOverall()

double mpc_local_planner::TimeSeriesSE2::computeMeanOverall ( )
overridevirtual

Compute and return the mean value of all values among all dimensions.

Reimplemented from corbo::TimeSeries.

Definition at line 134 of file time_series_se2.cpp.

◆ getValuesInterpolate()

bool mpc_local_planner::TimeSeriesSE2::getValuesInterpolate ( double  time,
Eigen::Ref< Eigen::VectorXd >  values,
Interpolation  interpolation = Interpolation::Linear,
Extrapolation  extrapolate = Extrapolation::NoExtrapolation,
double  tolerance = 1e-6 
) const
overridevirtual

Retrieve value vector at a desired time stamp (seconds) via interpolation.

This method interpolates the underlying time series object at a desired time stamp (given in seconds). Interpolation and extrapolation settings can be passed as optional arguments.

Warning
This method currently assumes that the values / time pairs are stored with monotonically increasing time stamps (in time()).
Parameters
[in]timedesired time stamp
[out]valuesinterpolated value vector w.r.t. time
[in]interpolationinterpolation method according to Interpolation enum
[in]extrapolatespecify whether exrapliation should be applied with a similar method like interpolation
[in]tolerancespecify a tolerance for which two time stamps are treated equally
Returns
true if successfull, false otherwise (e.g. if extrapolate is false but time > max(time)

Reimplemented from corbo::TimeSeries.

Definition at line 54 of file time_series_se2.cpp.


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


mpc_local_planner
Author(s): Christoph Rösmann
autogenerated on Wed Mar 2 2022 00:35:06