SkillManagerComponentPlugin.cpp

Go to the documentation of this file.

#include "SkillManagerComponentPlugin.h"
 
#include <ArmarXCore/core/Component.h>
#include <ArmarXCore/core/time/DateTime.h>
#include <ArmarXCore/core/time/ice_conversions.h>
 
#include <RobotAPI/libraries/skills/core/SkillID.h>
#include <RobotAPI/libraries/skills/core/SkillStatusUpdate.h>
#include <RobotAPI/libraries/skills/core/error/Exception.h>
 
namespace armarx::plugins
{
    void
    SkillManagerComponentPlugin::preOnInitComponent()
    {
    }
 
    void
    SkillManagerComponentPlugin::preOnConnectComponent()
    {
        auto& p = parent<SkillManagerComponentPluginUser>();
        p.getProxy(myPrx, -1);
    }
 
    void
    SkillManagerComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
    {
    }
 
    skills::ProviderID
    SkillManagerComponentPlugin::getFirstProviderNameThatHasSkill(const skills::SkillID& skillId)
    {
        // NON LOCKING! WE ASSERT THAT THE CALLER HOLDS LOCK
        for (const auto& [providerName, providerPrx] : skillProviderMap)
        {
            auto allSkills = providerPrx->getSkillDescriptions();
            for (const auto& [currentSkillID, skillDesc] : allSkills)
            {
                if (currentSkillID.skillName == skillId.skillName)
                {
                    return {providerName};
                }
            }
        }
        return {"INVALID PROVIDER NAME"};
    }
 
    void
    SkillManagerComponentPlugin::addProvider(const skills::ProviderInfo& providerInfo)
    {
        std::scoped_lock l(skillProviderMapMutex);
        if (skillProviderMap.find(providerInfo.providerId) == skillProviderMap.end())
        {
            ARMARX_INFO << "Adding a provider with name '" << providerInfo.providerId.providerName
                        << "'.";
            skillProviderMap.insert({providerInfo.providerId, providerInfo.providerInterface});
        }
        else
        {
            ARMARX_INFO << "Trying to add a provider with name '"
                        << providerInfo.providerId.providerName
                        << "' but the provider already exists. "
                        << "Overwriting the old provider info.";
            skillProviderMap[providerInfo.providerId] = providerInfo.providerInterface;
        }
    }
 
    void
    SkillManagerComponentPlugin::removeProvider(const skills::ProviderID& providerId)
    {
        std::scoped_lock l(skillProviderMapMutex);
        if (auto it = skillProviderMap.find(providerId); it != skillProviderMap.end())
        {
            ARMARX_INFO << "Removing a provider with name '" << providerId.providerName << "'.";
            skillProviderMap.erase(it);
        }
        else
        {
            ARMARX_INFO << "Trying to remove a provider with name '" << providerId.providerName
                        << "' but it couldn't be found.";
        }
    }
 
    skills::SkillStatusUpdate
    SkillManagerComponentPlugin::executeSkill(const skills::SkillExecutionRequest& executionRequest)
    {
        ARMARX_CHECK(executionRequest.skillId.isFullySpecified())
            << "Got: " << executionRequest.skillId.toString();
 
        std::unique_lock l(skillProviderMapMutex);
 
        skills::ProviderID provderId(*executionRequest.skillId.providerId);
 
        // TODO: Really support regexes!
        if (executionRequest.skillId.providerId->providerName == "*")
        {
            provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
        }
 
 
        if (auto it = skillProviderMap.find(provderId); it != skillProviderMap.end())
        {
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << provderId << "'. Removing it from skills.";
                skillProviderMap.erase(it);
 
                throw skills::error::SkillException(
                    __PRETTY_FUNCTION__,
                    "Skill execution failed. Could not execute a skill of provider '" +
                        provderId.toString() + "' because the provider does not exist.");
            }
 
            try
            {
                skills::SkillExecutionRequest provider_executionRequest{
                    .skillId = executionRequest.skillId,
                    .executorName = executionRequest.executorName,
                    .parameters = executionRequest.parameters,
                    .callbackInterface = myPrx};
 
                auto async =
                    provider->begin_executeSkill(provider_executionRequest.toProviderIce());
                l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
                auto provider_statusUpdate_ice = provider->end_executeSkill(async);
 
                // convert to manager view
                auto statusUpdate =
                    skills::SkillStatusUpdate::FromIce(provider_statusUpdate_ice, provderId);
                return statusUpdate;
            }
            catch (const std::exception &e)
            {
                l.lock();
 
                handleExceptionNonLockingThrow(__PRETTY_FUNCTION__, e, provderId);
            }
        }
        else
        {
            throw skills::error::SkillException(
                __PRETTY_FUNCTION__,
                "Skill execution failed. Could not execute a skill of provider '" +
                    provderId.toString() + "' because the provider does not exist.");
        }
    }
 
    skills::SkillExecutionID
    SkillManagerComponentPlugin::executeSkillAsync(
        const skills::SkillExecutionRequest& executionRequest)
    {
        ARMARX_CHECK(executionRequest.skillId.isFullySpecified())
            << "Got: " << executionRequest.skillId.toString();
 
        std::unique_lock l(skillProviderMapMutex);
 
        skills::ProviderID provderId(*executionRequest.skillId.providerId);
 
        // TODO: Really support regexes!
        if (executionRequest.skillId.providerId->providerName == "*")
        {
            provderId = getFirstProviderNameThatHasSkill(executionRequest.skillId);
        }
 
 
        if (auto it = skillProviderMap.find(provderId); it != skillProviderMap.end())
        {
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << provderId << "'. Removing it from skills.";
                skillProviderMap.erase(it);
 
                throw skills::error::SkillException(
                    __PRETTY_FUNCTION__,
                    "Skill execution failed. Could not execute a skill of provider '" +
                        provderId.toString() + "' because the provider does not exist.");
            }
 
            try
            {
                skills::SkillExecutionRequest provider_executionRequest{
                    .skillId = executionRequest.skillId,
                    .executorName = executionRequest.executorName,
                    .parameters = executionRequest.parameters,
                    .callbackInterface = myPrx};
 
                auto async =
                    provider->begin_executeSkillAsync(provider_executionRequest.toProviderIce());
                l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
                auto provider_executionID_ice = provider->end_executeSkillAsync(async);
 
                // convert to manager view
                auto executionId =
                    skills::SkillExecutionID::FromIce(provider_executionID_ice, provderId);
                executionId.skillId.providerId = provderId;
                return executionId;
            }
            catch (const std::exception &e)
            {
                l.lock();
 
                handleExceptionNonLockingThrow(__PRETTY_FUNCTION__, e, provderId);
            }
        }
        else
        {
            throw skills::error::SkillException(
                __PRETTY_FUNCTION__,
                "Skill execution failed. Could not execute a skill of provider '" +
                    provderId.toString() + "' because the provider does not exist.");
        }
    }
 
    bool
    SkillManagerComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
                                                       const aron::data::DictPtr& data)
    {
        ARMARX_DEBUG << "updateSkillParameters for skill " << executionId.skillId;
 
        ARMARX_CHECK(executionId.skillId.isFullySpecified())
            << "Got: " << executionId.skillId.toString();
 
        std::unique_lock l(skillProviderMapMutex);
        if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
            it != skillProviderMap.end())
        {
            const auto& providerId = it->first;
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << providerId << "'. Removing it from skills.";
                skillProviderMap.erase(it);
                return false;
            }
 
            try
            {
                auto async = provider->begin_updateSkillParameters(executionId.toProviderIce(),
                                                                   data->toAronDictDTO());
                l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
                auto r = provider->end_updateSkillParameters(async);
                return r.success;
            }
            catch (const std::exception &e)
            {
                l.lock();
 
                handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId);
 
                return false;
            }
        }
        else
        {
            return false;
        }
    }
 
    bool
    SkillManagerComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
    {
        ARMARX_DEBUG << "abortSkill for skill " << executionId.skillId;
 
        ARMARX_CHECK(executionId.skillId.isFullySpecified())
            << "Got: " << executionId.skillId.toString();
 
        std::unique_lock l(skillProviderMapMutex);
        if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
            it != skillProviderMap.end())
        {
            const auto& providerId = it->first;
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << providerId << "'. Removing it from skills.";
                skillProviderMap.erase(it);
                return false;
            }
 
            try
            {
                auto async = provider->begin_abortSkill(executionId.toProviderIce());
                l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
                auto r = provider->end_abortSkill(async);
                return r.success;
            }
            catch (const std::exception &e)
            {
                l.lock();
 
                handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId);
 
                return false;
            }
        }
        else
        {
            return false;
        }
    }
 
    bool
    SkillManagerComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
    {
        ARMARX_CHECK(executionId.skillId.isFullySpecified())
            << "Got: " << executionId.skillId.toString();
 
        std::unique_lock l(skillProviderMapMutex);
        if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
            it != skillProviderMap.end())
        {
            const auto& providerId = it->first;
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << providerId << "'. Removing it from skills.";
                skillProviderMap.erase(it);
                return false;
            }
 
            try
            {
                auto async = provider->begin_abortSkill(executionId.toProviderIce());
                return true;
            }
            catch (const std::exception &e)
            {
                handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId);
 
                return false;
            }
        }
        else
        {
            return false;
        }
    }
 
    std::optional<skills::SkillDescription>
    SkillManagerComponentPlugin::getSkillDescription(const skills::SkillID& skillId)
    {
        ARMARX_DEBUG << "getSkillDescription for skill " << skillId;
 
        ARMARX_CHECK(skillId.isFullySpecified()) << "Got: " << skillId.toString();
 
        std::unique_lock l(skillProviderMapMutex);
        if (auto it = skillProviderMap.find(*skillId.providerId); it != skillProviderMap.end())
        {
            const auto& providerId = it->first;
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << providerId << "'. Removing it from skills.";
                skillProviderMap.erase(it);
 
                throw skills::error::SkillException(__PRETTY_FUNCTION__,
                                                    "Skill execution failed. Could not query a "
                                                    "status update of a skill of provider '" +
                                                        providerId.toString() +
                                                        "' because the provider does not exist.");
            }
 
            try
            {
                auto async = provider->begin_getSkillDescription(skillId.toProviderIce());
                l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
                auto provider_desc_ice = provider->end_getSkillDescription(async);
 
                if (not provider_desc_ice)
                {
                    return std::nullopt;
                }
 
                // convert to manager view
                auto desc =
                    skills::SkillDescription::FromIce(provider_desc_ice.value(), providerId);
                return desc;
            }
            catch (const std::exception &e)
            {
                handleExceptionNonLockingThrow(__PRETTY_FUNCTION__, e, providerId);
            }
        }
        else
        {
            return std::nullopt;
        }
    }
 
    std::map<skills::SkillID, skills::SkillDescription>
    SkillManagerComponentPlugin::getSkillDescriptions()
    {
        std::map<skills::SkillID, skills::SkillDescription> ret;
 
        std::scoped_lock l(skillProviderMapMutex);
        for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
        {
            const auto& providerId = it->first;
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << providerId << "'. Removing it from skills.";
                it = skillProviderMap.erase(it);
                continue;
            }
 
            try
            {
                skills::provider::dto::SkillDescriptionMap m = provider->getSkillDescriptions();
 
                for (const auto& [provider_skillId_ice, skillDescription_ice] : m)
                {
                    ret.insert(
                        {skills::SkillID::FromIce(provider_skillId_ice, providerId),
                         skills::SkillDescription::FromIce(skillDescription_ice, providerId)});
                }
 
                ++it;
            }
            catch (const std::exception &e)
            {
                if (auto _it = handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId))
                {
                    it = _it.value(); // next element
                }
            }
        }
        return ret;
    }
 
    std::optional<skills::SkillStatusUpdate>
    SkillManagerComponentPlugin::getSkillExecutionStatus(
        const skills::SkillExecutionID& executionId)
    {
        ARMARX_CHECK(executionId.skillId.isFullySpecified())
            << "Got: " << executionId.skillId.toString();
 
        std::unique_lock l(skillProviderMapMutex);
        if (auto it = skillProviderMap.find(*executionId.skillId.providerId);
            it != skillProviderMap.end())
        {
            const auto& providerId = it->first;
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << providerId << "'. Removing it from skills.";
                skillProviderMap.erase(it);
 
                throw skills::error::SkillException(__PRETTY_FUNCTION__,
                                                    "Skill execution failed. Could not query a "
                                                    "status update of a skill of provider '" +
                                                        providerId.toString() +
                                                        "' because the provider does not exist.");
            }
 
            try
            {
                auto async = provider->begin_getSkillExecutionStatus(executionId.toProviderIce());
                l.unlock(); // allow parallel e.g. stopping. Otherwise the manager would lock himself in nested calls
                auto provider_statusUpdate_ice = provider->end_getSkillExecutionStatus(async);
 
                if (not provider_statusUpdate_ice)
                {
                    return std::nullopt;
                }
 
                // convert to manager view
                auto statusUpdate = skills::SkillStatusUpdate::FromIce(
                    provider_statusUpdate_ice.value(), providerId);
                return statusUpdate;
            }
            catch (const std::exception &e)
            {
                l.lock();
 
                handleExceptionNonLockingThrow(__PRETTY_FUNCTION__, e, providerId);
            }
        }
        else
        {
            return std::nullopt;
        }
    }
 
    std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
    SkillManagerComponentPlugin::getSkillExecutionStatuses()
    {
        std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> ret;
 
        std::scoped_lock l(skillProviderMapMutex);
        for (auto it = skillProviderMap.cbegin(); it != skillProviderMap.cend();)
        {
            const auto& providerId = it->first;
            const auto& provider = it->second;
 
            if (!provider)
            {
                ARMARX_WARNING << __PRETTY_FUNCTION__ << ": Found disconnected skill provider '"
                               << providerId << "'. Removing it from skills.";
                it = skillProviderMap.erase(it);
                continue;
            }
 
            try
            {
                auto m = provider->getSkillExecutionStatuses();
 
                for (const auto& [provider_executionId_ice, provider_statusUpdate_ice] : m)
                {
                    ret.insert(
                        {skills::SkillExecutionID::FromIce(provider_executionId_ice, providerId),
                         skills::SkillStatusUpdate::FromIce(provider_statusUpdate_ice,
                                                            providerId)});
                }
                it++;
            }
            catch (const std::exception &e)
            {
                if (auto _it = handleExceptionNonLocking(__PRETTY_FUNCTION__, e, providerId))
                {
                    it = _it.value(); // next element
                }
            }
        }
        return ret;
    }
 
    std::optional<std::map<skills::ProviderID, skills::provider::dti::SkillProviderInterfacePrx>::iterator>
    SkillManagerComponentPlugin::handleExceptionNonLocking(
        const char* funcName, const std::exception& e, skills::ProviderID providerId, bool eraseSkillProvider)
    {
        // NON LOCKING! WE ASSERT THAT THE CALLER HOLDS LOCK
        if (auto it = skillProviderMap.find(providerId); eraseSkillProvider and it != skillProviderMap.end())
        {
            ARMARX_WARNING << funcName
                        << ": Found disconnected or buggy skill provider '" << providerId
                        << "' during execution. Removing it from skills. "
                        << "Error: " << e.what();
            return skillProviderMap.erase(it);
        }
        else
        {
            ARMARX_WARNING << funcName
                        << ": Found disconnected or buggy skill provider '" << providerId
                        << "' during execution. However, it already got removed... "
                        << "Error: " << e.what();
            return std::nullopt;
        }
    }
 
    void 
    SkillManagerComponentPlugin::handleExceptionNonLockingThrow(
        const char* funcName, const std::exception& e, skills::ProviderID providerId, bool eraseSkillProvider)
    {
        // NON LOCKING! WE ASSERT THAT THE CALLER HOLDS LOCK
        handleExceptionNonLocking(funcName, e, providerId, eraseSkillProvider);
 
        throw skills::error::SkillException(
            funcName,
            "Skill execution failed. Could not execute a skill of provider '" +
                providerId.toString() + "' because the provider does not exist.");
    }
 
} // namespace armarx::plugins
 
namespace armarx
{
    SkillManagerComponentPluginUser::SkillManagerComponentPluginUser()
    {
        addPlugin(plugin);
    }
 
    void
    SkillManagerComponentPluginUser::addProvider(const skills::manager::dto::ProviderInfo& info,
                                                 const Ice::Current&)
    {
        auto i = skills::ProviderInfo::FromIce(info);
        this->plugin->addProvider(i);
    }
 
    void
    SkillManagerComponentPluginUser::removeProvider(
        const skills::manager::dto::ProviderID& provider,
        const Ice::Current&)
    {
        auto i = skills::ProviderID::FromIce(provider);
        this->plugin->removeProvider(i);
    }
 
    skills::manager::dto::SkillStatusUpdate
    SkillManagerComponentPluginUser::executeSkill(
        const skills::manager::dto::SkillExecutionRequest& info,
        const Ice::Current&)
    {
        auto e = skills::SkillExecutionRequest::FromIce(info);
        return this->plugin->executeSkill(e).toManagerIce();
    }
 
    skills::manager::dto::SkillExecutionID
    SkillManagerComponentPluginUser::executeSkillAsync(
        const skills::manager::dto::SkillExecutionRequest& info,
        const Ice::Current& current)
    {
        auto e = skills::SkillExecutionRequest::FromIce(info);
        return this->plugin->executeSkillAsync(e).toManagerIce();
    }
 
    skills::provider::dto::ParameterUpdateResult
    SkillManagerComponentPluginUser::updateSkillParameters(
        const skills::manager::dto::SkillExecutionID& info,
        const aron::data::dto::DictPtr& params,
        const Ice::Current& current)
    {
        skills::provider::dto::ParameterUpdateResult ret;
        auto a = armarx::aron::data::Dict::FromAronDictDTO(params);
        auto e = skills::SkillExecutionID::FromIce(info);
        ret.success = this->plugin->updateSkillParameters(e, a);
        return ret;
    }
 
    skills::provider::dto::AbortSkillResult
    SkillManagerComponentPluginUser::abortSkill(const skills::manager::dto::SkillExecutionID& id,
                                                const Ice::Current& current)
    {
        skills::provider::dto::AbortSkillResult ret;
        auto i = skills::SkillExecutionID::FromIce(id);
        ret.success = this->plugin->abortSkill(i);
        return ret;
    }
 
    skills::provider::dto::AbortSkillResult
    SkillManagerComponentPluginUser::abortSkillAsync(
        const skills::manager::dto::SkillExecutionID& id,
        const Ice::Current& /*unused*/)
    {
        skills::provider::dto::AbortSkillResult ret;
        auto i = skills::SkillExecutionID::FromIce(id);
        ret.success = this->plugin->abortSkillAsync(i);
        return ret;
    }
 
    void
    SkillManagerComponentPluginUser::updateStatusForSkill(
        const skills::provider::dto::SkillStatusUpdate& statusUpdate,
        const skills::callback::dto::ProviderID& pid,
        const Ice::Current&)
    {
        (void)statusUpdate;
        (void)pid;
        // If you want to use the status, implement this method!
    }
 
    skills::manager::dto::SkillDescriptionMap
    SkillManagerComponentPluginUser::getSkillDescriptions(const Ice::Current& current)
    {
        skills::manager::dto::SkillDescriptionMap ret;
 
        auto m = this->plugin->getSkillDescriptions();
 
        for (const auto& [k, v] : m)
        {
            ret.insert({k.toManagerIce(), v.toManagerIce()});
        }
 
        return ret;
    }
 
    IceUtil::Optional<skills::manager::dto::SkillDescription>
    SkillManagerComponentPluginUser::getSkillDescription(const skills::manager::dto::SkillID& id,
                                                         const Ice::Current& current)
    {
        auto e = skills::SkillID::FromIce(id);
        auto o = this->plugin->getSkillDescription(e);
        if (o.has_value())
        {
            return o->toManagerIce();
        }
        return {};
    }
 
    IceUtil::Optional<skills::manager::dto::SkillStatusUpdate>
    SkillManagerComponentPluginUser::getSkillExecutionStatus(
        const skills::manager::dto::SkillExecutionID& executionId,
        const Ice::Current& current)
    {
        auto e = skills::SkillExecutionID::FromIce(executionId);
        auto o = this->plugin->getSkillExecutionStatus(e);
        if (o.has_value())
        {
            return o->toManagerIce();
        }
        return {};
    }
 
    skills::manager::dto::SkillStatusUpdateMap
    SkillManagerComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& current)
    {
        skills::manager::dto::SkillStatusUpdateMap ret;
 
        auto m = this->plugin->getSkillExecutionStatuses();
 
        for (const auto& [k, v] : m)
        {
            ret.insert({k.toManagerIce(), v.toManagerIce()});
        }
 
        return ret;
    }
} // namespace armarx