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_