SkillProviderComponentPlugin.cpp

Go to the documentation of this file.

#include "SkillProviderComponentPlugin.h"
 
#include <ArmarXCore/core/Component.h>
 
#include <RobotAPI/libraries/aron/core/data/variant/primitive/All.h>
#include <RobotAPI/libraries/skills/core/ProviderInfo.h>
 
namespace armarx::plugins
{
    void
    SkillProviderComponentPlugin::preOnInitComponent()
    {
    }
 
    void
    SkillProviderComponentPlugin::preOnConnectComponent()
    {
        auto& p = parent<SkillProviderComponentPluginUser>();
        p.getProxy(myPrx, -1);
    }
 
    void
    SkillProviderComponentPlugin::postOnConnectComponent()
    {
        auto& p = parent<SkillProviderComponentPluginUser>();
        const std::string providerName = p.getName();
 
        // register self to manager
        skills::ProviderInfo i{.providerId = skills::ProviderID{.providerName = providerName},
                               .providerInterface = myPrx,
                               .providedSkills = getSkillDescriptions()};
 
        ARMARX_INFO << "Adding provider to manager: " << i.providerId;
        manager->addProvider(i.toIce()); // add provider info to manager
    }
 
    void
    SkillProviderComponentPlugin::preOnDisconnectComponent()
    {
        auto& p = parent<SkillProviderComponentPluginUser>();
        std::string providerName = p.getName();
 
        auto id = skills::manager::dto::ProviderID{providerName};
        manager->removeProvider(id);
    }
 
    void
    SkillProviderComponentPlugin::postCreatePropertyDefinitions(PropertyDefinitionsPtr& properties)
    {
        std::string prefix = "skill.";
        properties->component(
            manager,
            "SkillMemory",
            prefix + "SkillManager",
            "The name of the SkillManager (or SkillMemory) proxy this provider belongs to.");
    }
 
    void
    SkillProviderComponentPlugin::addSkillFactory(std::unique_ptr<skills::SkillBlueprint>&& fac)
    {
        if (!fac)
        {
            return;
        }
 
        auto& p = parent<SkillProviderComponentPluginUser>();
        const std::string componentName = p.getName();
 
        const skills::ProviderID providerId({componentName});
 
        // lock skills map
        const std::unique_lock l(skillFactoriesMutex);
        auto skillId = fac->createSkillDescription(providerId).skillId;
 
        if (skillFactories.find(skillId) != skillFactories.end())
        {
            ARMARX_WARNING << "Try to add a skill factory for skill '" + skillId.toString() +
                                  "' which already exists in list. Ignoring this skill.";
            return;
        }
 
        ARMARX_INFO << "Adding skill `" << skillId << "` to component `" << componentName << "` .";
 
        skillFactories.emplace(skillId, std::move(fac));
 
        //        if (connected)
        //        {
        //            // if skill is added after onConnect we have to set the proxies manually.
        //            std::string providerName = parent().getName();
        //            s.first->second.skill->manager = manager;
        //            s.first->second.skill->providerName = providerName;
        //        }
    }
 
    skills::SkillBlueprint*
    SkillProviderComponentPlugin::addSkillFactory(const skills::SkillDescription& desc,
                                                  const skills::LambdaSkill::FunctionType& f)
    {
        return addSkillFactory<skills::LambdaSkill>(desc, f);
    }
 
    skills::SkillBlueprint*
    SkillProviderComponentPlugin::getSkillFactory(const armarx::skills::SkillID& skillId)
    {
        // NON BLOCKING: WE ASSERT THAT THE LOCK IS ALREADY TAKEN
        ARMARX_CHECK(skillId.isFullySpecified());
 
        if (skillFactories.count(skillId) == 0)
        {
            ARMARX_INFO << "Could not find a skill factory for id: " << skillId;
            return nullptr;
        }
 
        auto* facPtr = skillFactories.at(skillId).get();
        return static_cast<skills::SkillBlueprint*>(facPtr);
    }
 
    std::optional<skills::SkillStatusUpdate>
    SkillProviderComponentPlugin::getSkillExecutionStatus(
        const skills::SkillExecutionID& execId) const
    {
        ARMARX_CHECK(execId.skillId.isSkillSpecified());
 
        const std::shared_lock l(skillExecutionsMutex);
        if (skillExecutions.find(execId) == skillExecutions.end())
        {
            ARMARX_WARNING << "Skill execution for skill '" + execId.skillId.toString() +
                                  "' not found!";
            return std::nullopt;
        }
 
        return skillExecutions.at(execId).statusUpdate;
    }
 
    std::map<skills::SkillExecutionID, skills::SkillStatusUpdate>
    SkillProviderComponentPlugin::getSkillExecutionStatuses() const
    {
        std::map<skills::SkillExecutionID, skills::SkillStatusUpdate> skillUpdates;
 
        const std::shared_lock l(skillExecutionsMutex);
        for (const auto& [key, impl] : skillExecutions)
        {
            const std::scoped_lock l2(impl.skillStatusesMutex);
            skillUpdates.insert({key, impl.statusUpdate});
        }
        return skillUpdates;
    }
 
    std::optional<skills::SkillDescription>
    SkillProviderComponentPlugin::getSkillDescription(const skills::SkillID& skillId) const
    {
        ARMARX_CHECK(skillId.isFullySpecified());
 
        const std::shared_lock l(skillFactoriesMutex);
        if (skillFactories.find(skillId) == skillFactories.end())
        {
            std::stringstream ss;
            ss << "Skill description for skill '" + skillId.toString() +
                      "' not found! Found instead: {"
               << "\n";
            for (const auto& [k, _] : skillFactories)
            {
                ss << "\t" << k.toString() << "\n";
            }
            ss << "}";
            ARMARX_WARNING << ss.str();
 
            return std::nullopt;
        }
 
        return skillFactories.at(skillId)->createSkillDescription(*skillId.providerId);
    }
 
    std::map<skills::SkillID, skills::SkillDescription>
    SkillProviderComponentPlugin::getSkillDescriptions() const
    {
        std::map<skills::SkillID, skills::SkillDescription> skillDesciptions;
        const std::shared_lock l(skillFactoriesMutex);
        for (const auto& [key, fac] : skillFactories)
        {
            ARMARX_CHECK(key.isFullySpecified());
            skillDesciptions.insert({key, fac->createSkillDescription(*key.providerId)});
        }
        return skillDesciptions;
    }
 
    skills::SkillStatusUpdate
    SkillProviderComponentPlugin::executeSkill(
        const skills::SkillExecutionRequest& executionRequest)
    {
        ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
 
        skills::SkillExecutionID executionId{.skillId = executionRequest.skillId,
                                             .executorName = executionRequest.executorName,
                                             .executionStartedTime =
                                                 armarx::core::time::DateTime::Now()};
 
        skills::SkillStatusUpdate ret{
            {executionId, executionRequest.parameters, executionRequest.callbackInterface}};
 
        skills::detail::SkillRuntime* wrapper;
        {
            auto l1 = std::unique_lock{skillFactoriesMutex};
 
            const auto& fac = getSkillFactory(executionId.skillId);
            ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionId.skillId;
 
            {
                const std::unique_lock l2{skillExecutionsMutex};
                auto it =
                    skillExecutions.emplace(std::piecewise_construct,
                                            std::make_tuple(executionId),
                                            std::make_tuple(fac,
                                                            executionId,
                                                            executionRequest.parameters,
                                                            executionRequest.callbackInterface));
                wrapper = &it.first->second;
                // ATTENTION NOT DEFINED BEHAVIOR
            }
 
            // async start execution. But we wait for the execution to finish at the end of this method
            wrapper->execution = std::thread(
                [&]()
                {
                    // execute waits until the previous execution finishes.
                    auto x = wrapper->executeSkill();
                    ret.result = x.result;
                    ret.status = armarx::skills::toSkillStatus(x.status);
                });
        } // release lock. We don't know how long the skill needs to finish and we have to release the lock for being able to abort the execution
 
        if (wrapper && wrapper->execution.joinable())
        {
            wrapper->execution.join();
 
            // tidy up map of executions
            //            const std::unique_lock l2{skillExecutionsMutex};
            //            if (auto it = skillExecutions.find(executionId); it != skillExecutions.end())
            //            {
            //                skillExecutions.erase(it);
            //            }
        }
        return ret;
    }
 
    skills::SkillExecutionID
    SkillProviderComponentPlugin::executeSkillAsync(
        const skills::SkillExecutionRequest& executionRequest)
    {
        ARMARX_CHECK(executionRequest.skillId.isFullySpecified());
 
        skills::SkillExecutionID executionId{executionRequest.skillId,
                                             executionRequest.executorName,
                                             armarx::core::time::DateTime::Now()};
 
        skills::detail::SkillRuntime* wrapper;
        {
            auto l1 = std::unique_lock{skillFactoriesMutex};
 
            const auto& fac = getSkillFactory(executionId.skillId);
            ARMARX_CHECK(fac) << "Could not find a factory for skill " << executionId.skillId;
 
            {
                const std::unique_lock l2{skillExecutionsMutex};
                auto it =
                    skillExecutions.emplace(std::piecewise_construct,
                                            std::make_tuple(executionId),
                                            std::make_tuple(fac,
                                                            executionId,
                                                            executionRequest.parameters,
                                                            executionRequest.callbackInterface));
                wrapper = &it.first->second;
                // ATTENTION NOT DEFINED BEHAVIOR
            }
 
            wrapper->execution = std::thread(
                [&]()
                {
                    // execute waits until the previous execution finishes.
                    auto x = wrapper->executeSkill();
                });
        }
 
        // wait until skill is constructed. This assures, that a status update exists.
        while (true)
        {
            {
                std::scoped_lock l(wrapper->skillStatusesMutex);
 
                if (wrapper->statusUpdate.hasBeenConstructed())
                {
                    break;
                }
            }
 
            std::this_thread::sleep_for(std::chrono::milliseconds(20));
        }
 
        return executionId;
    }
 
    bool
    SkillProviderComponentPlugin::updateSkillParameters(const skills::SkillExecutionID& executionId,
                                                        const armarx::aron::data::DictPtr& input)
    {
        ARMARX_CHECK(executionId.skillId.isFullySpecified());
 
        std::shared_lock l{skillExecutionsMutex};
        auto it = skillExecutions.find(executionId);
        if (it == skillExecutions.end())
        {
            ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
                               "' found! Ignoring prepareSkill request.";
            return false;
        }
 
        std::scoped_lock l2{it->second.skillStatusesMutex};
        if (it->second.statusUpdate.status != skills::SkillStatus::Preparing)
        {
            ARMARX_INFO << "Could not prepare the skill '" + executionId.skillId.toString() +
                               "' because its not in preparing phase.";
            return false;
        }
 
        it->second.updateSkillParameters(input);
        return true;
    }
 
    bool
    SkillProviderComponentPlugin::abortSkill(const skills::SkillExecutionID& executionId)
    {
        ARMARX_CHECK(executionId.skillId.isFullySpecified());
 
        std::shared_lock l(skillExecutionsMutex);
        auto it = skillExecutions.find(executionId);
        if (it == skillExecutions.end())
        {
            ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
                               "' found! Ignoring abortSkill request.";
            return false;
        }
 
        auto& runtime = it->second;
        runtime.stopSkill();
 
        while (true)
        {
            {
                std::scoped_lock l(runtime.skillStatusesMutex);
                auto status = runtime.statusUpdate;
 
                if (status.hasBeenTerminated())
                {
                    break;
                }
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(20));
        }
 
        return true;
    }
 
    bool
    SkillProviderComponentPlugin::abortSkillAsync(const skills::SkillExecutionID& executionId)
    {
        ARMARX_CHECK(executionId.skillId.isFullySpecified());
 
        std::shared_lock l(skillExecutionsMutex);
        auto it = skillExecutions.find(executionId);
        if (it == skillExecutions.end())
        {
            ARMARX_INFO << "No acive execution for skill '" + executionId.skillId.toString() +
                               "' found! Ignoring abortSkill request.";
            return false;
        }
 
        it->second.stopSkill();
        return true;
    }
 
} // namespace armarx::plugins
 
namespace armarx
{
    SkillProviderComponentPluginUser::SkillProviderComponentPluginUser()
    {
        addPlugin(plugin);
    }
 
    IceUtil::Optional<skills::provider::dto::SkillDescription>
    SkillProviderComponentPluginUser::getSkillDescription(
        const skills::provider::dto::SkillID& skillId,
        const Ice::Current& /*unused*/)
    {
        auto id = skills::SkillID::FromIce(skillId, skills::ProviderID{.providerName = getName()});
        auto o = plugin->getSkillDescription(id);
        if (o.has_value())
        {
            return o->toProviderIce();
        }
        return {};
    }
 
    skills::provider::dto::SkillDescriptionMap
    SkillProviderComponentPluginUser::getSkillDescriptions(const Ice::Current& /*unused*/)
    {
        skills::provider::dto::SkillDescriptionMap ret;
        for (const auto& [k, v] : plugin->getSkillDescriptions())
        {
            ret.insert({k.toProviderIce(), v.toProviderIce()});
        }
        return ret;
    }
 
    IceUtil::Optional<skills::provider::dto::SkillStatusUpdate>
    SkillProviderComponentPluginUser::getSkillExecutionStatus(
        const skills::provider::dto::SkillExecutionID& executionId,
        const Ice::Current& /*unused*/)
    {
        auto execId = skills::SkillExecutionID::FromIce(
            executionId, skills::ProviderID{.providerName = getName()});
        auto o = plugin->getSkillExecutionStatus(execId);
        if (o.has_value())
        {
            return o->toProviderIce();
        }
        return {};
    }
 
    skills::provider::dto::SkillStatusUpdateMap
    SkillProviderComponentPluginUser::getSkillExecutionStatuses(const Ice::Current& /*unused*/)
    {
        skills::provider::dto::SkillStatusUpdateMap ret;
        for (const auto& [k, v] : plugin->getSkillExecutionStatuses())
        {
            ret.insert({k.toProviderIce(), v.toProviderIce()});
        }
        return ret;
    }
 
    // Please not that this method waits until the skill can be scheduled!
    skills::provider::dto::SkillStatusUpdate
    SkillProviderComponentPluginUser::executeSkill(
        const skills::provider::dto::SkillExecutionRequest& info,
        const Ice::Current& /*unused*/)
    {
        auto exec = skills::SkillExecutionRequest::FromIce(
            info, skills::ProviderID{.providerName = getName()});
        auto up = this->plugin->executeSkill(exec);
        return up.toProviderIce();
    }
 
    skills::provider::dto::SkillExecutionID
    SkillProviderComponentPluginUser::executeSkillAsync(
        const skills::provider::dto::SkillExecutionRequest& info,
        const Ice::Current& current /*unused*/)
    {
        auto exec = skills::SkillExecutionRequest::FromIce(
            info, skills::ProviderID{.providerName = getName()});
        auto id = this->plugin->executeSkillAsync(exec);
        return id.toProviderIce();
    }
 
    skills::provider::dto::ParameterUpdateResult
    SkillProviderComponentPluginUser::updateSkillParameters(
        const skills::provider::dto::SkillExecutionID& id,
        const aron::data::dto::DictPtr& input,
        const Ice::Current& current /*unused*/)
    {
        skills::provider::dto::ParameterUpdateResult res;
 
        auto exec =
            skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
        auto prep = armarx::aron::data::Dict::FromAronDictDTO(input);
        res.success = this->plugin->updateSkillParameters(exec, prep);
        return res;
    }
 
    skills::provider::dto::AbortSkillResult
    SkillProviderComponentPluginUser::abortSkill(const skills::provider::dto::SkillExecutionID& id,
                                                 const Ice::Current& /*unused*/)
    {
        skills::provider::dto::AbortSkillResult res;
        auto exec =
            skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
        res.success = this->plugin->abortSkill(exec);
        return res;
    }
 
    skills::provider::dto::AbortSkillResult
    SkillProviderComponentPluginUser::abortSkillAsync(
        const skills::provider::dto::SkillExecutionID& id,
        const Ice::Current& /*unused*/)
    {
        skills::provider::dto::AbortSkillResult res;
        auto exec =
            skills::SkillExecutionID::FromIce(id, skills::ProviderID{.providerName = getName()});
        res.success = this->plugin->abortSkillAsync(exec);
        return res;
    }
 
    const std::experimental::observer_ptr<plugins::SkillProviderComponentPlugin>&
    SkillProviderComponentPluginUser::getSkillProviderPlugin() const
    {
        return plugin;
    }
} // namespace armarx