ControllerHandler.h

Go to the documentation of this file.

#pragma once
 
#include <filesystem>
#include <memory>
#include <string>
 
#include <ArmarXCore/core/ManagedIceObject.h>
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/control/client/ControllerDescription.h>
#include <armarx/control/client/ControllerWrapper.h>
#include <armarx/view_selection/common/controller_types.h>
#include <armarx/view_selection/gaze_controller/gaze_ik/aron_conversions.h>
#include <armarx/view_selection/gaze_scheduler/ControllerHandlerInterface.h>
#include <armarx/view_selection/gaze_targets/GazeTarget.h>
#include <armarx/view_selection/gaze_targets/aron/GazeTarget.aron.generated.h>
#include <armarx/view_selection/gaze_targets/aron_conversions.h>
 
namespace armarx::view_selection::gaze_scheduler
{
    struct ControllerHandlerInjectedServices
    {
        std::experimental::observer_ptr<armarx::control::client::ComponentPlugin>
            controlComponentPlugin;
    };
 
    struct ControllerHandlerParams
    {
        std::string robotName;
    };
 
    template <armarx::view_selection::common::ControllerType ControllerT>
    class ControllerHandler : virtual public ControllerHandlerInterface
    {
    public:
        using InjectedServices = ControllerHandlerInjectedServices;
        using Params = ControllerHandlerParams;
 
        ControllerHandler(const InjectedServices& srv, const Params& params) :
            srv(srv), params(params)
        {
        }
 
        ~ControllerHandler() override
        {
            // ctrl.reset();
        }
 
        void
        activateController() override
        {
            // Properties are loaded OK
            ARMARX_INFO << "Activating controller";
            ctrl->activate();
        }
 
        void
        createController() override
        {
            // initialize controller
            ARMARX_INFO << "Initializing controller";
            {
                ARMARX_TRACE;
                auto builder = srv.controlComponentPlugin->createControllerBuilder<ControllerT>();
                builder.withNodeSet("Head");
 
                const std::string controllerTypeID =
                    control::client::ControllerDescription<ControllerT>::name;
 
                // Load the default or robot-specific config
                {
                    // prefer robot-specific config if available
                    const armarx::PackagePath robotSpecificConfigPath(
                        "armarx_view_selection",
                        "controller_config/" + controllerTypeID + "/" + params.robotName + ".json");
 
                    if (std::filesystem::exists(robotSpecificConfigPath.toSystemPath()))
                    {
                        ARMARX_INFO << "Using robot specific config: " << robotSpecificConfigPath;
                        builder.withConfig(robotSpecificConfigPath.toSystemPath());
                    }
                    else // fallback: use default config
                    {
                        const armarx::PackagePath defaultConfigPath(
                            "armarx_view_selection",
                            "controller_config/" + controllerTypeID + "/default.json");
 
                        ARMARX_INFO << "Using default config: " << defaultConfigPath;
 
                        ARMARX_CHECK(std::filesystem::exists(defaultConfigPath.toSystemPath()))
                            << defaultConfigPath;
 
                        ARMARX_VERBOSE << "builder.withNodeSet().withConfig";
                        builder.withConfig(defaultConfigPath.toSystemPath());
                    }
                }
 
                ARMARX_VERBOSE << "builder.create()";
                auto ctrlWrapper = builder.create();
                ARMARX_TRACE;
                ARMARX_CHECK_NOT_NULL(ctrlWrapper);
 
                ARMARX_VERBOSE << "Successfully created controller";
                ctrl = std::move(ctrlWrapper);
            }
 
            ARMARX_TRACE;
            ARMARX_CHECK_NOT_NULL(ctrl);
        }
 
        void
        updateControllerTarget(const gaze_targets::GazeTarget& gazeTarget) override
        {
            ARMARX_VERBOSE << "Sending new target to the controller " << gazeTarget;
            armarx::view_selection::gaze_targets::arondto::GazeTarget dto;
            toAron(dto, gazeTarget);
 
            if (not ctrl->ctrl()->isControllerActive())
            {
                ARMARX_INFO << "Activating controller because it is not active yet.";
                ctrl->ctrl()->activateController();
                armarx::Clock::WaitFor(armarx::Duration::MilliSeconds(500));
            }
 
            ctrl->config.target = dto;
            ctrl->updateConfig();
        }
 
    protected:
    private:
        std::unique_ptr<armarx::control::client::ControllerWrapper<ControllerT>> ctrl;
 
        const InjectedServices srv;
 
        const Params params;
    };
} // namespace armarx::view_selection::gaze_scheduler