simulated_plant_threaded.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_PLANTS_INCLUDE_CORBO_PLANTS_SIMULATED_PLANT_THREADED_H_
#define SRC_PLANTS_INCLUDE_CORBO_PLANTS_SIMULATED_PLANT_THREADED_H_

#include <corbo-plants/simulated_plant.h>

#include <memory>
#include <mutex>
#include <thread>

namespace corbo {

class SimulatedPlantThreaded : public SimulatedPlant
{
 public:
    using StateVector = Eigen::VectorXd;

    SimulatedPlantThreaded();
    SimulatedPlantThreaded(SystemDynamicsInterface::Ptr dynamics, SystemOutputInterface::Ptr output);

    virtual ~SimulatedPlantThreaded();

    // implements interface method
    Ptr getInstance() const override { return std::make_shared<SimulatedPlantThreaded>(); }

    // implements interface method
    bool initialize() override;

    // implements interface method
    void stop() override;

    // implements interface method
    void reset() override;

    // implements interface method
    bool control(const TimeSeries::ConstPtr& u_sequence, const TimeSeries::ConstPtr& x_sequence, const Duration& dt, const Time& t,
                 SignalTargetInterface* signal_target = nullptr, const std::string& ns = "") override;

    // implements interface method
    bool output(OutputVector& output, const Time& t, SignalTargetInterface* signal_target = nullptr, const std::string& ns = "") override;

    // implements interface method
    bool setState(const Eigen::Ref<const Eigen::VectorXd>& state) override;

    void setSimulationRate(double sim_rate) { _sim_rate = sim_rate; }

#ifdef MESSAGE_SUPPORT
    void toMessage(messages::SimulatedPlantThreaded& message) const;
    void fromMessage(const messages::SimulatedPlantThreaded& message, std::stringstream* issues = nullptr);

    // implements interface method
    void toMessage(messages::Plant& message) const override { toMessage(*message.mutable_simulated_plant_threaded()); }
    // implements interface method
    void fromMessage(const messages::Plant& message, std::stringstream* issues = nullptr) override
    {
        fromMessage(message.simulated_plant_threaded(), issues);
    }
#endif

 protected:
    void simulate();

 private:
    double _sim_rate = 10;

    std::thread _worker_thread;
    bool _stop_thread = false;

    std::mutex _control_mutex;
    ControlVector _control;

    std::mutex _current_state_mutex;
};

FACTORY_REGISTER_PLANT(SimulatedPlantThreaded)

}  // namespace corbo

#endif  // SRC_PLANTS_INCLUDE_CORBO_PLANTS_SIMULATED_PLANT_THREADED_H_