ControllerBase.h

/*+-------------------------------------------------------------------------+
  |                       MultiVehicle simulator (libmvsim)                 |
  |                                                                         |
  | Copyright (C) 2014-2026  Jose Luis Blanco Claraco                       |
  | Copyright (C) 2017  Borys Tymchenko (Odessa Polytechnic University)     |
  | Distributed under 3-clause BSD License                                  |
  |   See COPYING                                                           |
  +-------------------------------------------------------------------------+ */
 
#pragma once
 
#include <mvsim/basic_types.h>
 
#include <optional>
 
namespace mvsim
{
class ControllerBaseInterface
{
   public:
    struct TeleopInput
    {
        int keycode = 0;
        std::optional<TJoyStickEvent> js;
 
        TeleopInput() = default;
    };
 
    struct TeleopOutput
    {
        std::string append_gui_lines;
    };
 
    virtual void teleop_interface(
        [[maybe_unused]] const TeleopInput& in, [[maybe_unused]] TeleopOutput& out)
    {
        /*default: do nothing*/
    }
 
    virtual bool setTwistCommand([[maybe_unused]] const mrpt::math::TTwist2D& t)
    {
        return false; /* default: no */
    }
 
    virtual bool getTrajectoryPlotPoints(
        [[maybe_unused]] std::vector<mrpt::math::TPoint2D>& pts,
        [[maybe_unused]] double& height) const
    {
        return false; /* default: no trajectory data */
    }
};
 
template <class VEH_DYNAMICS>
class ControllerBaseTempl : public ControllerBaseInterface
{
   public:
    using Ptr = std::shared_ptr<ControllerBaseTempl<VEH_DYNAMICS>>;
 
    ControllerBaseTempl(VEH_DYNAMICS& veh) : veh_(veh) {}
    virtual ~ControllerBaseTempl() {}
    virtual void teleop_interface(const TeleopInput& in, TeleopOutput& out) override
    {
        using namespace std::string_literals;
 
        /*default: handle logging events*/
        static bool isRecording = false;
        switch (in.keycode)
        {
            case 'l':
            case 'L':
            {
                isRecording = !isRecording;
                setLogRecording(isRecording);
            }
            break;
            case 'c':
            case 'C':
            {
                clearLogs();
            }
            break;
 
            case 'n':
            case 'N':
            {
                newLogSession();
            }
            break;
 
            default:
                break;
        }
 
        out.append_gui_lines += "\nToggle logging [L]. Clear logs[C]. New session [N].\n";
        out.append_gui_lines += "Now logging: "s + (isRecording ? "YES\n"s : "NO\n"s);
    }
 
    virtual void control_step(
        const typename VEH_DYNAMICS::TControllerInput& ci,
        typename VEH_DYNAMICS::TControllerOutput& co) = 0;
 
    virtual void on_post_step([[maybe_unused]] const TSimulContext& context) {}
 
    virtual void load_config(  //
        [[maybe_unused]] const rapidxml::xml_node<char>& node)
    { /*default: do nothing*/
    }
 
    virtual void setLogRecording(bool recording) { veh_.setRecording(recording); }
    virtual void clearLogs() { veh_.clearLogs(); }
    virtual void newLogSession() { veh_.newLogSession(); }
 
   protected:
    VEH_DYNAMICS& veh_;
};
}  // namespace mvsim