ControllerBuilder.h

Go to the documentation of this file.

/**
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX 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 <http://www.gnu.org/licenses/>.
 *
 * @author     Fabian Reister ( fabian dot reister at kit dot edu )
 * @date       2022
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include <filesystem>
#include <optional>
#include <fstream>
#include <type_traits>
#include <SimoxUtility/json/json.hpp>
 
#include "ArmarXCore/core/exceptions/LocalException.h"
#include "ArmarXCore/core/time/Clock.h"
#include "RobotAPI/libraries/aron/core/data/rw/reader/nlohmannJSON/NlohmannJSONReaderWithoutTypeCheck.h"
#include <RobotAPI/interface/units/RobotUnit/RobotUnitInterface.h>
 
#include <armarx/control/client/ControllerWrapper.h>
#include <armarx/control/client/ControllerDescription.h>
#include <armarx/control/common/type.h>
#include <armarx/control/memory/config/Writer.h>
#include <armarx/control/memory/config/Reader.h>
#include <armarx/control/memory/constants.h>
#include <armarx/control/interface/ConfigurableNJointControllerInterface.h>
#include <armarx/control/core/Controller.h>
 
#include <RobotAPI/libraries/aron/core/codegeneration/cpp/AronGeneratedClass.h>
 
namespace armarx::control::client
{
 
 
    template <auto T>
    class ControllerBuilder //: public AbstractControllerBuilder
    {
    public:
        using ControllerDescriptionType = ControllerDescription<T>;
 
        // static constexpr auto ControllerType = ct;
        using AronDTO = typename ControllerDescriptionType::AronDTO;
        // using AronType = AronDTO;
 
        static_assert(
            std::is_base_of<armarx::aron::codegenerator::cpp::AronGeneratedClass, AronDTO>::value,
            "asdf");
 
        ControllerBuilder(const armarx::RobotUnitInterfacePrx& controllerCreator,
                          const memory::config::Reader& configProvider,
                          const memory::config::Writer& configWriter,
                          const std::string& controllerNamePrefix) :
            controllerCreator(controllerCreator),
            configReader(configProvider),
            configWriter(configWriter),
            controllerNamePrefix(controllerNamePrefix)
        {
            ARMARX_CHECK_NOT_EMPTY(controllerNamePrefix)
                << "By convention, the controller name prefix must exist. This ensures that "
                   "in between components, no naming clashes occur.";
        }
 
        ControllerBuilder&
        withDefaultConfig(const std::string& name = "default")
        {
            if (auto cfg = configReader.getDefaultConfig(ControllerDescriptionType::name, name))
            {
                dto = AronDTO::FromAron(cfg);
            }
            else
            {
                ARMARX_WARNING << "Failed to obtain default config";
            }
 
            return *this;
        }
 
        ControllerBuilder&
        withConfig(const armarx::armem::MemoryID& memoryId)
        {
            auto cfg = configReader.getConfig(memoryId);
            dto = AronDTO::FromAron(cfg);
 
            return *this;
        }
 
        ControllerBuilder&
        withConfig(const std::filesystem::path& filename)
        {
            ARMARX_CHECK(std::filesystem::is_regular_file(filename)) << filename;
 
            ARMARX_INFO << "Loading config from file `" << filename << "`.";
            std::ifstream ifs{filename};
 
            nlohmann::json jsonConfig;
            ifs >> jsonConfig;
 
            ARMARX_INFO << "Initializing config";
            if(not this->dto.has_value())
            {
                this->dto = AronDTO();
            }
 
            armarx::aron::data::reader::NlohmannJSONReaderWithoutTypeCheck reader;
 
            ARMARX_CHECK(this->dto.has_value());
 
            ARMARX_INFO << "reading file.";
            this->dto->read(reader, jsonConfig);
 
            return *this;
        }
 
        ControllerBuilder&
        withConfig(const AronDTO& dto)
        {
            this->dto = dto;
            return *this;
        }
 
        AronDTO&
        config()
        {
            ensureInitialized();
 
            return dto.value();
        }
 
        ControllerBuilder&
        withNodeSet(const std::string& nodeSetName)
        {
            this->nodeSetName = nodeSetName;
            return *this;
        }
 
        ControllerBuilder&
        withNamePrefix(const std::string& namePrefix)
        {
            if (namePrefix.empty())
                return *this;
            this->controllerNamePrefix = this->controllerNamePrefix + "_" + namePrefix;
            return *this;
        }
 
        /**
         * @brief Creates the controller with the default name
         * 
         * @return std::optional<ControllerWrapper<AronDTO, ct>> 
         */
        std::optional<ControllerWrapper<T>>
        create()
        {
            const std::string controllerClassName = ControllerDescriptionType::name;
            return create(controllerClassName);
        }
 
 
        std::optional<ControllerWrapper<T>>
        create(const std::string& name)
        {
            if (not ensureInitialized())
            {
                return std::nullopt;
            }
 
            ::armarx::control::ConfigurableNJointControllerConfigPtr config =
                new ::armarx::control::ConfigurableNJointControllerConfig;
            config->config = dto.value().toAronDTO();
 
            if (not nodeSetName.has_value())
            {
                ARMARX_WARNING << "You forgot to set the node set name!";
                return std::nullopt;
            }
            config->nodeSetName = nodeSetName.value();
 
            controllerName = controllerNamePrefix + "_" + nodeSetName.value() + "_" + name;
            const std::string controllerClassName = ControllerDescriptionType::name;
 
            // reuse or create controller
 
            armarx::NJointControllerInterfacePrx controller =
                controllerCreator->getNJointController(controllerName);
 
 
            const bool canReuseController = [&]() -> bool
            {
                if (not controller)
                {
                    ARMARX_INFO << "Controller does not exist yet.";
                    return false;
                }
 
                if(not allowReuse_)
                {
                    ARMARX_INFO << "Not allowed to reuse controller.";
                    return false;
                }
 
                // check if controller has correct type
                if (controller->getClassName() != controllerClassName)
                {
                    ARMARX_INFO << "Controller class does not match.";
                    return false;
                }
 
                // TODO: check if kinematic chains match
                // if (controller->getNodeSetName() != nodeSetName)
                // {
                //     return false;
                // }
 
                return true;
            }();
 
            if((not allowReuse_) and controller)
            {
                ARMARX_TRACE;
 
                ARMARX_INFO << "Deleting existing controller `" << controllerName << "`";
                try
                {
                    ARMARX_TRACE;
                    controllerCreator->deactivateAndDeleteNJointController(controllerName);
                }
                catch(...)
                {
                    ARMARX_WARNING << GetHandledExceptionString();
                }
 
                ARMARX_INFO << "Done deleting";
 
                Clock::WaitFor(Duration::MilliSeconds(150));
            }
 
            if((not allowReuse_) and controller)
            {
                ARMARX_TRACE;
 
                ARMARX_INFO << "Deleting existing controller `" << controllerName << "`";
                try
                {
                    ARMARX_TRACE;
                    controllerCreator->deactivateAndDeleteNJointController(controllerName);
                }
                catch(...)
                {
                    ARMARX_WARNING << GetHandledExceptionString();
                }
 
                ARMARX_INFO << "Done deleting";
 
                Clock::WaitFor(Duration::MilliSeconds(150));
            }
 
            // create controller if necessary
            if (not canReuseController)
            {
                ARMARX_TRACE;
 
                ARMARX_INFO << "Creating controller `" << controllerName << "`";
 
                controller = controllerCreator->createNJointController(
                    controllerClassName, controllerName, config);
                ARMARX_CHECK_NOT_NULL(controller);
            }
 
            if (not controller)
            {
                ARMARX_WARNING << "Failed to create controller of type `" << controllerClassName
                               << "`";
                return std::nullopt;
            }
 
            auto ctrl =
                armarx::control::ConfigurableNJointControllerInterfacePrx::checkedCast(controller);
 
            auto ctrlWrapper =
                ControllerWrapper<T>(ctrl, controllerName, configWriter, dto.value());
 
            // Sends the data to the memory and updates the controller config.
            // This is especially important, if the controller is reused.
            ctrlWrapper.updateConfig();
 
            return std::move(ctrlWrapper);
        }
 
        ControllerBuilder&  allowReuse(const bool allowReuse)
        {
            this->allowReuse_ = allowReuse;
            return *this;
        }
 
        std::string getControllerName()
        {
            return controllerName;
        }
 
    private:
        bool
        initDefaultConfig()
        {
            if (auto cfg = configReader.getDefaultConfig(ControllerDescriptionType::name))
            {
                dto = AronDTO::FromAron(cfg);
                return true;
            }
 
            ARMARX_WARNING << "Failed to obtain default config";
            return false;
        }
 
        bool
        ensureInitialized()
        {
            if (not dto.has_value())
            {
                return initDefaultConfig();
            }
 
            return true;
        }
 
 
        armarx::RobotUnitInterfacePrx controllerCreator;
 
        const memory::config::Reader configReader;
        const memory::config::Writer configWriter;
 
        std::optional<AronDTO> dto;
 
        std::optional<std::string> nodeSetName;
 
        std::string controllerNamePrefix;
        std::string controllerName;
 
        bool allowReuse_ = false;
    };
} // namespace armarx::control::client