signals.h

Go to the documentation of this file.

/*********************************************************************
 *
 *  Software License Agreement
 *
 *  Copyright (c) 2020,
 *  TU Dortmund - Institute of Control Theory and Systems Engineering.
 *  All rights reserved.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <https://www.gnu.org/licenses/>.
 *
 *  Authors: Christoph Rösmann
 *********************************************************************/
 
#ifndef SRC_CORE_INCLUDE_CORBO_CORE_SIGNALS_H_
#define SRC_CORE_INCLUDE_CORBO_CORE_SIGNALS_H_
 
#include <corbo-core/console.h>
#include <corbo-core/time.h>
#include <corbo-core/time_series.h>
#include <corbo-core/types.h>
 
#ifdef MESSAGE_SUPPORT
#include <corbo-communication/messages/core/signals.pb.h>
#endif
 
#include <map>
#include <memory>
#include <string>
#include <type_traits>
#include <unordered_map>
#include <vector>
 
namespace corbo {
 
constexpr const char SIGNAL_NAMESPACE_DELIMITER = '/';
 
enum class SignalType {
    TimeSeries,
    Measurement,
    Values,
    TimeSeriesSequence,
    IndexedValues,
    IndexedValuesSet,
    LabeledValues,
    LabeledValuesSet,
    Matrix,
    MatrixSet
};
 
struct SignalHeader
{
    std::string name = "unknown";  
    Time time;                     
    int value_dimension  = 0;      
    bool zero_order_hold = false;  
 
    std::string getShortName() const
    {
        std::size_t found = name.find_last_of("/");
        return name.substr(found + 1);
    }
 
#ifdef MESSAGE_SUPPORT
    void toMessage(corbo::messages::SignalHeader& message) const
    {
        message.set_time(time.toSec());
        message.set_name(name);
        message.set_value_dimension(value_dimension);
        message.set_zero_order_hold(zero_order_hold);
    }
    void fromMessage(const corbo::messages::SignalHeader& message, std::stringstream* issues = nullptr)
    {
        time.fromSec(message.time());
        name            = message.name();
        value_dimension = message.value_dimension();
        zero_order_hold = message.zero_order_hold();
    }
#endif
};
 
class SignalInterface
{
 public:
    using Ptr      = std::shared_ptr<SignalInterface>;
    using ConstPtr = std::shared_ptr<const SignalInterface>;
 
    virtual ~SignalInterface() {}
 
    virtual SignalType getType() const = 0;
 
    virtual void getValueLabels(std::vector<std::string>& sublabels) const {}
 
    SignalHeader header;
 
#ifdef MESSAGE_SUPPORT
    virtual void toMessage(corbo::messages::Signal& message) const {}
    virtual void fromMessage(const corbo::messages::Signal& message, std::stringstream* issues = nullptr) {}
#endif
};
 
class Measurement : public SignalInterface
{
 public:
    using Ptr      = std::shared_ptr<Measurement>;
    using ConstPtr = std::shared_ptr<const Measurement>;
 
    Measurement() {}
    Measurement(double time, const std::vector<double>& values) : _time(time), _values(values) {}
    Measurement(double time, const Eigen::Ref<const Eigen::VectorXd>& values) { set(time, values); }
 
    // Implements interface method
    SignalType getType() const override { return SignalType::Measurement; }
 
    void set(double time, const std::vector<double>& values)
    {
        _time   = time;
        _values = values;
    }
 
    void set(double time, const Eigen::Ref<const Eigen::VectorXd>& values)
    {
        _time = time;
        _values.assign(values.data(), values.data() + values.size());
    }
 
    const std::vector<double>& getValues() const { return _values; }
    std::vector<double>& getValuesRef() { return _values; }
 
    double getTime() const { return _time; }
 
    const std::vector<std::string>& getValueLabels() const { return _value_labels; }
    std::vector<std::string>& getValueLabelsRef() { return _value_labels; }
 
    // Implements interface method
    void getValueLabels(std::vector<std::string>& sublabels) const override { sublabels = _value_labels; }
 
#ifdef MESSAGE_SUPPORT
    // Implements interface method
    void toMessage(corbo::messages::Signal& message) const override;
    // Implements interface method
    void fromMessage(const corbo::messages::Signal& message, std::stringstream* issues = nullptr) override;
#endif
 
 private:
    double _time;                            
    std::vector<double> _values;             
    std::vector<std::string> _value_labels;  
};
 
class TimeSeriesSignal : public SignalInterface
{
 public:
    using Ptr      = std::shared_ptr<TimeSeriesSignal>;
    using ConstPtr = std::shared_ptr<const TimeSeriesSignal>;
 
    TimeSeriesSignal() : _time_series(new TimeSeries) {}
    explicit TimeSeriesSignal(int value_dim) : _time_series(new TimeSeries(value_dim)) {}
    explicit TimeSeriesSignal(TimeSeries::Ptr time_series) : _time_series(time_series) {}
    explicit TimeSeriesSignal(const Measurement& measurment) { add(measurment); }
 
    bool isEmpty() const { return _time_series ? _time_series->isEmpty() : true; }
 
    // Implements interface method
    SignalType getType() const override { return SignalType::TimeSeries; }
 
    // Implements interface method
    void getValueLabels(std::vector<std::string>& sublabels) const override
    {
        if (_time_series) sublabels = _time_series->getValueLabels();
    }
 
    void add(const Measurement& measurement);
    void add(double time, const Eigen::Ref<const Eigen::VectorXd>& values);
    void add(double time, const std::vector<double>& values);
 
    void set(TimeSeries::Ptr time_series) { _time_series = time_series; }
 
    void set(const Eigen::Ref<const Eigen::VectorXd>& time, const Eigen::Ref<const Eigen::MatrixXd>& values_matrix, double time_from_start = 0.0);
 
    void setTimeFromStart(double time_from_start);
 
    const TimeSeries* getTimeSeries() const { return _time_series.get(); }
    TimeSeries* getTimeSeriesRaw() { return _time_series.get(); }
 
    TimeSeries::Ptr getTimeSeriesPtr() const { return _time_series; }
 
#ifdef MESSAGE_SUPPORT
    // Implements interface method
    void toMessage(corbo::messages::Signal& message) const override;
    // Implements interface method
    void fromMessage(const corbo::messages::Signal& message, std::stringstream* issues = nullptr) override;
#endif
 
 private:
    TimeSeries::Ptr _time_series;
};
 
class TimeSeriesSequenceSignal : public SignalInterface
{
 public:
    using Ptr      = std::shared_ptr<TimeSeriesSequenceSignal>;
    using ConstPtr = std::shared_ptr<const TimeSeriesSequenceSignal>;
 
    TimeSeriesSequenceSignal() {}
    explicit TimeSeriesSequenceSignal(int value_dim) : _ts_sequence(new TimeSeriesSequence(value_dim)) {}
 
    bool isEmpty() const { return _ts_sequence ? _ts_sequence->isEmpty() : true; }
    // Implements interface method
    SignalType getType() const override { return SignalType::TimeSeriesSequence; }
    // Implements interface method
    void getValueLabels(std::vector<std::string>& sublabels) const override
    {
        // copy values from very first object in the sequence
        if (_ts_sequence && !_ts_sequence->isEmpty() && _ts_sequence->getSequence().front())
            sublabels = _ts_sequence->getSequence().front()->getValueLabels();
    }
 
    void add(TimeSeries::Ptr ts);
 
    void set(TimeSeriesSequence::Ptr ts_sequence) { _ts_sequence = ts_sequence; }
 
    const TimeSeriesSequence* getSequence() const { return _ts_sequence.get(); }
    TimeSeriesSequence* getSequenceRaw() { return _ts_sequence.get(); }
 
    TimeSeriesSequence::Ptr getSequencePtr() const { return _ts_sequence; }
 
#ifdef MESSAGE_SUPPORT
    // Implements interface method
    void toMessage(corbo::messages::Signal& message) const override;
    // Implements interface method
    void fromMessage(const corbo::messages::Signal& message, std::stringstream* issues = nullptr) override;
#endif
 
 private:
    TimeSeriesSequence::Ptr _ts_sequence;
};
 
class IndexedValuesSignal : public SignalInterface
{
 public:
    using Ptr      = std::shared_ptr<IndexedValuesSignal>;
    using ConstPtr = std::shared_ptr<const IndexedValuesSignal>;
 
    IndexedValuesSignal() = default;
    explicit IndexedValuesSignal(int index) : _index(index) {}
    IndexedValuesSignal(int index, double value) : _index(index) { add(value); }
    IndexedValuesSignal(int index, const std::vector<double>& values) : _index(index), _values(values) {}
    IndexedValuesSignal(int index, const Eigen::Ref<const Eigen::VectorXd>& values) : _index(index) { add(values); }
 
    bool isEmpty() const { return _values.empty(); }
 
    // Implements interface method
    SignalType getType() const override { return SignalType::IndexedValues; }
 
    // Get internal value dimension
    int getValueDimension() const { return _values.size(); }
 
    void setIndex(int index) { _index = index; }
 
    void add(double value) { _values.push_back(value); }
    void add(const Eigen::Ref<const Eigen::VectorXd>& values);
    void add(const std::vector<double>& values);
 
    void set(int index, double value);
    void set(int index, const Eigen::Ref<const Eigen::VectorXd>& values);
    void set(int index, const std::vector<double>& values);
 
    int getIndex() const { return _index; }
    const std::vector<double>& getValues() const { return _values; }
    std::vector<double>& getValuesRef() { return _values; }
 
    // Clear value vector
    void clear() { _values.clear(); }
 
#ifdef MESSAGE_SUPPORT
    void toMessage(corbo::messages::IndexedValues& message) const;
    void fromMessage(const corbo::messages::IndexedValues& message, std::stringstream* issues = nullptr);
    // Implements interface method
    void toMessage(corbo::messages::Signal& message) const override
    {
        header.toMessage(*message.mutable_header());
        toMessage(*message.mutable_indexed_values());
    }
    // Implements interface method
    void fromMessage(const corbo::messages::Signal& message, std::stringstream* issues = nullptr) override
    {
        header.fromMessage(message.header());
        fromMessage(message.indexed_values(), issues);
    }
#endif
 
 private:
    int _index = 0;
    std::vector<double> _values;
};
 
class IndexedValuesSetSignal : public SignalInterface
{
 public:
    using Ptr      = std::shared_ptr<IndexedValuesSetSignal>;
    using ConstPtr = std::shared_ptr<const IndexedValuesSetSignal>;
    using Map      = std::map<int, std::vector<double>>;
 
    IndexedValuesSetSignal() = default;
 
    bool isEmpty() const { return _values_map.empty(); }
 
    // Implements interface method
    SignalType getType() const override { return SignalType::IndexedValuesSet; }
 
    void add(int index, double value);
    void add(const IndexedValuesSignal& indexed_values);
    void add(int index, const Eigen::Ref<const Eigen::VectorXd>& values);
    void add(int index, const std::vector<double>& values);
 
    const Map& getData() const { return _values_map; }
    Map& getDataRef() { return _values_map; }
 
    int getMaxValueDimension() const;
 
    void clear() { _values_map.clear(); }
 
#ifdef MESSAGE_SUPPORT
    void toMessage(corbo::messages::IndexedValuesSet& message) const;
    void fromMessage(const corbo::messages::IndexedValuesSet& message, std::stringstream* issues = nullptr);
    // Implements interface method
    void toMessage(corbo::messages::Signal& message) const override
    {
        header.toMessage(*message.mutable_header());
        toMessage(*message.mutable_indexed_values_set());
    }
    // Implements interface method
    void fromMessage(const corbo::messages::Signal& message, std::stringstream* issues = nullptr) override
    {
        header.fromMessage(message.header());
        fromMessage(message.indexed_values_set(), issues);
    }
#endif
 
 private:
    Map _values_map;
};
 
class MatrixSignal : public SignalInterface
{
 public:
    using Ptr      = std::shared_ptr<MatrixSignal>;
    using ConstPtr = std::shared_ptr<const MatrixSignal>;
 
    MatrixSignal() = default;
    explicit MatrixSignal(const Eigen::Ref<const Eigen::MatrixXd>& matrix, const std::string& label = "") : _matrix(matrix), _label(label) {}
 
    bool isEmpty() const { return _matrix.rows() == 0 && _matrix.cols() == 0; }
 
    // Implements interface method
    SignalType getType() const override { return SignalType::Matrix; }
 
    int getRowDimension() const { return _matrix.rows(); }
    int getColDimension() const { return _matrix.cols(); }
 
    void set(const Eigen::Ref<const Eigen::MatrixXd>& matrix, const std::string& label = "")
    {
        _matrix = matrix;
        _label  = label;
    }
 
    const Eigen::MatrixXd& getMatrix() const { return _matrix; }
    Eigen::MatrixXd& getMatrixRef() { return _matrix; }
 
    const std::string& getLabel() const { return _label; }
    std::string& getLabelRef() { return _label; }
 
#ifdef MESSAGE_SUPPORT
    void toMessage(corbo::messages::Matrix& message) const;
    void fromMessage(const corbo::messages::Matrix& message, std::stringstream* issues = nullptr);
    // Implements interface method
    void toMessage(corbo::messages::Signal& message) const override
    {
        header.toMessage(*message.mutable_header());
        toMessage(*message.mutable_matrix());
    }
    // Implements interface method
    void fromMessage(const corbo::messages::Signal& message, std::stringstream* issues = nullptr) override
    {
        header.fromMessage(message.header());
        fromMessage(message.matrix(), issues);
    }
#endif
 
 private:
    Eigen::MatrixXd _matrix;
    std::string _label;
};
 
class MatrixSetSignal : public SignalInterface
{
 public:
    using Ptr      = std::shared_ptr<MatrixSetSignal>;
    using ConstPtr = std::shared_ptr<const MatrixSetSignal>;
    using Map      = std::map<int, std::vector<double>>;
 
    MatrixSetSignal() = default;
 
    bool isEmpty() const { return _matrix_set.empty(); }
 
    // Implements interface method
    SignalType getType() const override { return SignalType::MatrixSet; }
 
    void add(MatrixSignal::Ptr& matrix_signal);
    void add(const Eigen::Ref<const Eigen::MatrixXd>& matrix, const std::string& label = "");
 
    const std::vector<MatrixSignal::Ptr>& getData() const { return _matrix_set; }
    std::vector<MatrixSignal::Ptr>& getDataRef() { return _matrix_set; }
 
    void clear() { _matrix_set.clear(); }
 
#ifdef MESSAGE_SUPPORT
    void toMessage(corbo::messages::MatrixSet& message) const;
    void fromMessage(const corbo::messages::MatrixSet& message, std::stringstream* issues = nullptr);
    // Implements interface method
    void toMessage(corbo::messages::Signal& message) const override
    {
        header.toMessage(*message.mutable_header());
        toMessage(*message.mutable_matrix_set());
    }
    // Implements interface method
    void fromMessage(const corbo::messages::Signal& message, std::stringstream* issues = nullptr) override
    {
        header.fromMessage(message.header());
        fromMessage(message.matrix_set(), issues);
    }
#endif
 
 private:
    std::vector<MatrixSignal::Ptr> _matrix_set;
};
 
}  // namespace corbo
 
#endif  // SRC_CORE_INCLUDE_CORBO_CORE_SIGNALS_H_